<!DOCTYPE html PUBLIC "-//W3C//DTD XHTML 1.0 Transitional//EN" "http://www.w3.org/TR/xhtml1/DTD/xhtml1-transitional.dtd">
<html xmlns="http://www.w3.org/1999/xhtml">
<head>
<meta http-equiv="Content-Type" content="text/xhtml;charset=UTF-8"/>
<meta http-equiv="X-UA-Compatible" content="IE=9"/>
<meta name="generator" content="Doxygen 1.8.6"/>
<title>easykf-2.04: /home/fix_jer/SVN/easykf_trunk/trunk/easykf-2.04/src/ukf_sr_state_ndim.h Source File</title>
<link href="tabs.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="jquery.js"></script>
<script type="text/javascript" src="dynsections.js"></script>
<link href="search/search.css" rel="stylesheet" type="text/css"/>
<script type="text/javascript" src="search/search.js"></script>
<script type="text/javascript">
  $(document).ready(function() { searchBox.OnSelectItem(0); });
</script>
<link href="doxygen.css" rel="stylesheet" type="text/css" />
</head>
<body>
<div id="top"><!-- do not remove this div, it is closed by doxygen! -->
<div id="titlearea">
<table cellspacing="0" cellpadding="0">
 <tbody>
 <tr style="height: 56px;">
  <td style="padding-left: 0.5em;">
   <div id="projectname">easykf-2.04
   </div>
  </td>
 </tr>
 </tbody>
</table>
</div>
<!-- end header part -->
<!-- Generated by Doxygen 1.8.6 -->
<script type="text/javascript">
var searchBox = new SearchBox("searchBox", "search",false,'Search');
</script>
  <div id="navrow1" class="tabs">
    <ul class="tablist">
      <li><a href="index.html"><span>Main&#160;Page</span></a></li>
      <li><a href="namespaces.html"><span>Namespaces</span></a></li>
      <li><a href="annotated.html"><span>Classes</span></a></li>
      <li class="current"><a href="files.html"><span>Files</span></a></li>
      <li><a href="examples.html"><span>Examples</span></a></li>
      <li>
        <div id="MSearchBox" class="MSearchBoxInactive">
        <span class="left">
          <img id="MSearchSelect" src="search/mag_sel.png"
               onmouseover="return searchBox.OnSearchSelectShow()"
               onmouseout="return searchBox.OnSearchSelectHide()"
               alt=""/>
          <input type="text" id="MSearchField" value="Search" accesskey="S"
               onfocus="searchBox.OnSearchFieldFocus(true)" 
               onblur="searchBox.OnSearchFieldFocus(false)" 
               onkeyup="searchBox.OnSearchFieldChange(event)"/>
          </span><span class="right">
            <a id="MSearchClose" href="javascript:searchBox.CloseResultsWindow()"><img id="MSearchCloseImg" border="0" src="search/close.png" alt=""/></a>
          </span>
        </div>
      </li>
    </ul>
  </div>
  <div id="navrow2" class="tabs2">
    <ul class="tablist">
      <li><a href="files.html"><span>File&#160;List</span></a></li>
      <li><a href="globals.html"><span>File&#160;Members</span></a></li>
    </ul>
  </div>
<!-- window showing the filter options -->
<div id="MSearchSelectWindow"
     onmouseover="return searchBox.OnSearchSelectShow()"
     onmouseout="return searchBox.OnSearchSelectHide()"
     onkeydown="return searchBox.OnSearchSelectKey(event)">
<a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(0)"><span class="SelectionMark">&#160;</span>All</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(1)"><span class="SelectionMark">&#160;</span>Classes</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(2)"><span class="SelectionMark">&#160;</span>Namespaces</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(3)"><span class="SelectionMark">&#160;</span>Files</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(4)"><span class="SelectionMark">&#160;</span>Functions</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(5)"><span class="SelectionMark">&#160;</span>Variables</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(6)"><span class="SelectionMark">&#160;</span>Enumerations</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(7)"><span class="SelectionMark">&#160;</span>Enumerator</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(8)"><span class="SelectionMark">&#160;</span>Macros</a><a class="SelectItem" href="javascript:void(0)" onclick="searchBox.OnSelectItem(9)"><span class="SelectionMark">&#160;</span>Pages</a></div>

<!-- iframe showing the search results (closed by default) -->
<div id="MSearchResultsWindow">
<iframe src="javascript:void(0)" frameborder="0" 
        name="MSearchResults" id="MSearchResults">
</iframe>
</div>

<div id="nav-path" class="navpath">
  <ul>
<li class="navelem"><a class="el" href="dir_68267d1309a1af8e8297ef4c3efbcdba.html">src</a></li>  </ul>
</div>
</div><!-- top -->
<div class="header">
  <div class="headertitle">
<div class="title">ukf_sr_state_ndim.h</div>  </div>
</div><!--header-->
<div class="contents">
<a href="ukf__sr__state__ndim_8h.html">Go to the documentation of this file.</a><div class="fragment"><div class="line"><a name="l00001"></a><span class="lineno">    1</span>&#160;<span class="comment">/* ukf_sr_state_ndim.h</span></div>
<div class="line"><a name="l00002"></a><span class="lineno">    2</span>&#160;<span class="comment"> * </span></div>
<div class="line"><a name="l00003"></a><span class="lineno">    3</span>&#160;<span class="comment"> * Copyright (C) 2011-2014 Jeremy Fix</span></div>
<div class="line"><a name="l00004"></a><span class="lineno">    4</span>&#160;<span class="comment"> * </span></div>
<div class="line"><a name="l00005"></a><span class="lineno">    5</span>&#160;<span class="comment"> * This program is free software; you can redistribute it and/or modify</span></div>
<div class="line"><a name="l00006"></a><span class="lineno">    6</span>&#160;<span class="comment"> * it under the terms of the GNU General Public License as published by</span></div>
<div class="line"><a name="l00007"></a><span class="lineno">    7</span>&#160;<span class="comment"> * the Free Software Foundation; either version 3 of the License, or (at</span></div>
<div class="line"><a name="l00008"></a><span class="lineno">    8</span>&#160;<span class="comment"> * your option) any later version.</span></div>
<div class="line"><a name="l00009"></a><span class="lineno">    9</span>&#160;<span class="comment"> * </span></div>
<div class="line"><a name="l00010"></a><span class="lineno">   10</span>&#160;<span class="comment"> * This program is distributed in the hope that it will be useful, but</span></div>
<div class="line"><a name="l00011"></a><span class="lineno">   11</span>&#160;<span class="comment"> * WITHOUT ANY WARRANTY; without even the implied warranty of</span></div>
<div class="line"><a name="l00012"></a><span class="lineno">   12</span>&#160;<span class="comment"> * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU</span></div>
<div class="line"><a name="l00013"></a><span class="lineno">   13</span>&#160;<span class="comment"> * General Public License for more details.</span></div>
<div class="line"><a name="l00014"></a><span class="lineno">   14</span>&#160;<span class="comment"> * </span></div>
<div class="line"><a name="l00015"></a><span class="lineno">   15</span>&#160;<span class="comment"> * You should have received a copy of the GNU General Public License</span></div>
<div class="line"><a name="l00016"></a><span class="lineno">   16</span>&#160;<span class="comment"> * along with this program; if not, write to the Free Software</span></div>
<div class="line"><a name="l00017"></a><span class="lineno">   17</span>&#160;<span class="comment"> * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.</span></div>
<div class="line"><a name="l00018"></a><span class="lineno">   18</span>&#160;<span class="comment"> */</span></div>
<div class="line"><a name="l00019"></a><span class="lineno">   19</span>&#160;</div>
<div class="line"><a name="l00020"></a><span class="lineno"><a class="line" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">   20</a></span>&#160;<span class="preprocessor">#define DEBUGMODE true</span></div>
<div class="line"><a name="l00021"></a><span class="lineno">   21</span>&#160;<span class="preprocessor"></span></div>
<div class="line"><a name="l00022"></a><span class="lineno">   22</span>&#160;<span class="preprocessor">#ifndef UKF_SR_STATE_NDIM_H</span></div>
<div class="line"><a name="l00023"></a><span class="lineno">   23</span>&#160;<span class="preprocessor"></span><span class="preprocessor">#define UKF_SR_STATE_NDIM_H</span></div>
<div class="line"><a name="l00024"></a><span class="lineno">   24</span>&#160;<span class="preprocessor"></span></div>
<div class="line"><a name="l00025"></a><span class="lineno">   25</span>&#160;<span class="preprocessor">#include &lt;gsl/gsl_linalg.h&gt;</span> <span class="comment">// For the Cholesky decomposition</span></div>
<div class="line"><a name="l00026"></a><span class="lineno">   26</span>&#160;<span class="preprocessor">#include &lt;gsl/gsl_math.h&gt;</span></div>
<div class="line"><a name="l00027"></a><span class="lineno">   27</span>&#160;<span class="preprocessor">#include &lt;gsl/gsl_blas.h&gt;</span></div>
<div class="line"><a name="l00028"></a><span class="lineno">   28</span>&#160;</div>
<div class="line"><a name="l00029"></a><span class="lineno">   29</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="ukf__types_8h.html">ukf_types.h</a>&quot;</span></div>
<div class="line"><a name="l00030"></a><span class="lineno">   30</span>&#160;<span class="preprocessor">#include &quot;<a class="code" href="ukf__math_8h.html">ukf_math.h</a>&quot;</span></div>
<div class="line"><a name="l00031"></a><span class="lineno">   31</span>&#160;</div>
<div class="line"><a name="l00032"></a><span class="lineno"><a class="line" href="ukf__sr__state__ndim_8h.html#a2b6b76843e5a380b93a5cd4238a688a8">   32</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="ukf__sr__state__ndim_8h.html#a2b6b76843e5a380b93a5cd4238a688a8">print_mat</a>(<span class="keyword">const</span> gsl_matrix * A)</div>
<div class="line"><a name="l00033"></a><span class="lineno">   33</span>&#160;{</div>
<div class="line"><a name="l00034"></a><span class="lineno">   34</span>&#160;    <span class="keywordflow">for</span>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i &lt; A-&gt;size1 ; ++i)</div>
<div class="line"><a name="l00035"></a><span class="lineno">   35</span>&#160;    {</div>
<div class="line"><a name="l00036"></a><span class="lineno">   36</span>&#160;        <span class="keywordflow">for</span>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> j = 0 ; j &lt; A -&gt;size2 ; ++j)</div>
<div class="line"><a name="l00037"></a><span class="lineno">   37</span>&#160;            printf(<span class="stringliteral">&quot;%e &quot;</span>, gsl_matrix_get(A,i,j));</div>
<div class="line"><a name="l00038"></a><span class="lineno">   38</span>&#160;</div>
<div class="line"><a name="l00039"></a><span class="lineno">   39</span>&#160;        printf(<span class="stringliteral">&quot;\n&quot;</span>);</div>
<div class="line"><a name="l00040"></a><span class="lineno">   40</span>&#160;    }</div>
<div class="line"><a name="l00041"></a><span class="lineno">   41</span>&#160;}</div>
<div class="line"><a name="l00042"></a><span class="lineno">   42</span>&#160;</div>
<div class="line"><a name="l00043"></a><span class="lineno"><a class="line" href="ukf__sr__state__ndim_8h.html#a7cfcd827e44f3bbf3fff23fbac44dfc8">   43</a></span>&#160;<span class="keywordtype">void</span> <a class="code" href="ukf__sr__state__ndim_8h.html#a7cfcd827e44f3bbf3fff23fbac44dfc8">print_vec</a>(<span class="keyword">const</span> gsl_vector * A)</div>
<div class="line"><a name="l00044"></a><span class="lineno">   44</span>&#160;{</div>
<div class="line"><a name="l00045"></a><span class="lineno">   45</span>&#160;    <span class="keywordflow">for</span>(<span class="keywordtype">unsigned</span> <span class="keywordtype">int</span> i = 0 ; i &lt; A-&gt;size ; ++i)</div>
<div class="line"><a name="l00046"></a><span class="lineno">   46</span>&#160;    {</div>
<div class="line"><a name="l00047"></a><span class="lineno">   47</span>&#160;        printf(<span class="stringliteral">&quot;%e &quot;</span>, gsl_vector_get(A,i));</div>
<div class="line"><a name="l00048"></a><span class="lineno">   48</span>&#160;    }</div>
<div class="line"><a name="l00049"></a><span class="lineno">   49</span>&#160;    printf(<span class="stringliteral">&quot;\n&quot;</span>);</div>
<div class="line"><a name="l00050"></a><span class="lineno">   50</span>&#160;}</div>
<div class="line"><a name="l00051"></a><span class="lineno">   51</span>&#160;</div>
<div class="line"><a name="l00052"></a><span class="lineno">   52</span>&#160;<span class="keyword">namespace </span>ukf</div>
<div class="line"><a name="l00053"></a><span class="lineno">   53</span>&#160;{</div>
<div class="line"><a name="l00054"></a><span class="lineno">   54</span>&#160;</div>
<div class="line"><a name="l00059"></a><span class="lineno"><a class="line" href="namespaceukf_1_1srstate.html">   59</a></span>&#160;    <span class="keyword">namespace </span>srstate</div>
<div class="line"><a name="l00060"></a><span class="lineno">   60</span>&#160;    {</div>
<div class="line"><a name="l00065"></a><span class="lineno"><a class="line" href="namespaceukf_1_1srstate.html#aff3bcb491d70b295c47a7644038e7ed2">   65</a></span>&#160;        <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="namespaceukf_1_1srstate.html#aff3bcb491d70b295c47a7644038e7ed2">ukf_init</a>(<a class="code" href="structukf_1_1srstate_1_1ukf__param.html">ukf_param</a> &amp;p, <a class="code" href="structukf_1_1srstate_1_1ukf__state.html">ukf_state</a> &amp;s)</div>
<div class="line"><a name="l00066"></a><span class="lineno">   66</span>&#160;        {</div>
<div class="line"><a name="l00067"></a><span class="lineno">   67</span>&#160;            <span class="comment">// Parameters for the sigma points of the process equation</span></div>
<div class="line"><a name="l00068"></a><span class="lineno">   68</span>&#160;            p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a2a382b59ee2aefa913f3bc24dc70bb44">nbSamples</a> = 2 * p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + 1;</div>
<div class="line"><a name="l00069"></a><span class="lineno">   69</span>&#160;            p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#aedb04b04c2a74208f787e26eb3f406f5">lambda</a> = p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a55760e3aa010ff582f27067c6168972a">alpha</a> * p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a55760e3aa010ff582f27067c6168972a">alpha</a> * (p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a41231af7e574722fee0a575ce989e434">kpa</a>) - p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>;</div>
<div class="line"><a name="l00070"></a><span class="lineno">   70</span>&#160;            p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a5dc27e0d7ae329c6456efdeb0c359a39">gamma</a> = sqrt(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#aedb04b04c2a74208f787e26eb3f406f5">lambda</a>);</div>
<div class="line"><a name="l00071"></a><span class="lineno">   71</span>&#160;</div>
<div class="line"><a name="l00072"></a><span class="lineno">   72</span>&#160;            <span class="comment">// Parameters for the sigma points of the observation equation</span></div>
<div class="line"><a name="l00073"></a><span class="lineno">   73</span>&#160;            p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a45062e1fd58806a27f0e4f75772e33d2">nbSamplesMeasure</a> = 4 * p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + 1;</div>
<div class="line"><a name="l00074"></a><span class="lineno">   74</span>&#160;            p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a4802c63baa1362da178bd2456a47704b">lambda_aug</a> = p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a55760e3aa010ff582f27067c6168972a">alpha</a> * p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a55760e3aa010ff582f27067c6168972a">alpha</a> * (2*p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a41231af7e574722fee0a575ce989e434">kpa</a>) - 2*p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>;</div>
<div class="line"><a name="l00075"></a><span class="lineno">   75</span>&#160;            p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#aea53a3550db37bc15a3556df4c4edad8">gamma_aug</a> = sqrt(2*p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a4802c63baa1362da178bd2456a47704b">lambda_aug</a>);</div>
<div class="line"><a name="l00076"></a><span class="lineno">   76</span>&#160;</div>
<div class="line"><a name="l00077"></a><span class="lineno">   77</span>&#160;            <span class="comment">// Init the matrices used to iterate</span></div>
<div class="line"><a name="l00078"></a><span class="lineno">   78</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a9095ffca4f0bc5cbf7a42368cf7463d6">xi</a> = gsl_vector_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>);</div>
<div class="line"><a name="l00079"></a><span class="lineno">   79</span>&#160;            gsl_vector_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a9095ffca4f0bc5cbf7a42368cf7463d6">xi</a>);</div>
<div class="line"><a name="l00080"></a><span class="lineno">   80</span>&#160;</div>
<div class="line"><a name="l00081"></a><span class="lineno">   81</span>&#160;            <span class="comment">// Images of the sigma points</span></div>
<div class="line"><a name="l00082"></a><span class="lineno">   82</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0876b435bf2ca99901c779bde40d9004">xi_prediction</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a2a382b59ee2aefa913f3bc24dc70bb44">nbSamples</a>);</div>
<div class="line"><a name="l00083"></a><span class="lineno">   83</span>&#160;            gsl_matrix_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0876b435bf2ca99901c779bde40d9004">xi_prediction</a>);</div>
<div class="line"><a name="l00084"></a><span class="lineno">   84</span>&#160;</div>
<div class="line"><a name="l00085"></a><span class="lineno">   85</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0ead044a78987bebf3a2df35efdcead3">xi_mean</a> = gsl_vector_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>);</div>
<div class="line"><a name="l00086"></a><span class="lineno">   86</span>&#160;            gsl_vector_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0ead044a78987bebf3a2df35efdcead3">xi_mean</a>);</div>
<div class="line"><a name="l00087"></a><span class="lineno">   87</span>&#160;</div>
<div class="line"><a name="l00088"></a><span class="lineno">   88</span>&#160;            <span class="comment">// Cholesky factor of the variance/covariance matrix Pxxi</span></div>
<div class="line"><a name="l00089"></a><span class="lineno">   89</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">Sxi</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>,p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>);</div>
<div class="line"><a name="l00090"></a><span class="lineno">   90</span>&#160;            gsl_matrix_set_identity(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">Sxi</a>);</div>
<div class="line"><a name="l00091"></a><span class="lineno">   91</span>&#160;            gsl_matrix_scale(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">Sxi</a>, sqrt(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#adcc3839a78bea1cd4822dc10bea265c0">prior_x</a>));</div>
<div class="line"><a name="l00092"></a><span class="lineno">   92</span>&#160;</div>
<div class="line"><a name="l00093"></a><span class="lineno">   93</span>&#160;            <span class="comment">// Cholesky factor of the variance/covariance matrix of the process noise</span></div>
<div class="line"><a name="l00094"></a><span class="lineno">   94</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a51c541f3e8ba1379cb5f61876e17c375">Svi</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>,p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>);</div>
<div class="line"><a name="l00095"></a><span class="lineno">   95</span>&#160;            gsl_matrix_set_identity(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a51c541f3e8ba1379cb5f61876e17c375">Svi</a>);</div>
<div class="line"><a name="l00096"></a><span class="lineno">   96</span>&#160;            gsl_matrix_scale(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a51c541f3e8ba1379cb5f61876e17c375">Svi</a>, sqrt(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a91ac481e996d091fdb23db9eab23013e">process_noise</a>));</div>
<div class="line"><a name="l00097"></a><span class="lineno">   97</span>&#160;</div>
<div class="line"><a name="l00098"></a><span class="lineno">   98</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ac205dee75be43b32c76b0696d0182ab6">yi_prediction</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a45062e1fd58806a27f0e4f75772e33d2">nbSamplesMeasure</a>);</div>
<div class="line"><a name="l00099"></a><span class="lineno">   99</span>&#160;            gsl_matrix_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ac205dee75be43b32c76b0696d0182ab6">yi_prediction</a>);</div>
<div class="line"><a name="l00100"></a><span class="lineno">  100</span>&#160;</div>
<div class="line"><a name="l00101"></a><span class="lineno">  101</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#af0b49a1284aa6ce02d8fe249df41d434">yi_mean</a> = gsl_vector_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>);</div>
<div class="line"><a name="l00102"></a><span class="lineno">  102</span>&#160;            gsl_vector_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#af0b49a1284aa6ce02d8fe249df41d434">yi_mean</a>);</div>
<div class="line"><a name="l00103"></a><span class="lineno">  103</span>&#160;</div>
<div class="line"><a name="l00104"></a><span class="lineno">  104</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#add7a90f3736c8909a2e3abc3e4235ba7">ino_yi</a> = gsl_vector_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>);</div>
<div class="line"><a name="l00105"></a><span class="lineno">  105</span>&#160;            gsl_vector_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#add7a90f3736c8909a2e3abc3e4235ba7">ino_yi</a>);</div>
<div class="line"><a name="l00106"></a><span class="lineno">  106</span>&#160;</div>
<div class="line"><a name="l00107"></a><span class="lineno">  107</span>&#160;            <span class="comment">// Cholesky factor of the variance/covariance matrix Pyyi</span></div>
<div class="line"><a name="l00108"></a><span class="lineno">  108</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a70875ec1b32a4a73816c139ebe54d17e">Syi</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>);</div>
<div class="line"><a name="l00109"></a><span class="lineno">  109</span>&#160;            gsl_matrix_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a70875ec1b32a4a73816c139ebe54d17e">Syi</a>);</div>
<div class="line"><a name="l00110"></a><span class="lineno">  110</span>&#160;</div>
<div class="line"><a name="l00111"></a><span class="lineno">  111</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#afee48e0e83f20d7d46a478a9b436a42b">Sni</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>,p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>);</div>
<div class="line"><a name="l00112"></a><span class="lineno">  112</span>&#160;            gsl_matrix_set_identity(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#afee48e0e83f20d7d46a478a9b436a42b">Sni</a>);</div>
<div class="line"><a name="l00113"></a><span class="lineno">  113</span>&#160;            gsl_matrix_scale(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#afee48e0e83f20d7d46a478a9b436a42b">Sni</a>, sqrt(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a93c3981c8518153ce89bc868fb071267">measurement_noise</a>));</div>
<div class="line"><a name="l00114"></a><span class="lineno">  114</span>&#160;</div>
<div class="line"><a name="l00115"></a><span class="lineno">  115</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0d210360e59328e219b5f37751baac35">Pxyi</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>);</div>
<div class="line"><a name="l00116"></a><span class="lineno">  116</span>&#160;            gsl_matrix_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0d210360e59328e219b5f37751baac35">Pxyi</a>);</div>
<div class="line"><a name="l00117"></a><span class="lineno">  117</span>&#160;</div>
<div class="line"><a name="l00118"></a><span class="lineno">  118</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a698d2cd6e4da73ca0b4831e09f9c4419">sigmaPoint</a> = gsl_vector_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>);</div>
<div class="line"><a name="l00119"></a><span class="lineno">  119</span>&#160;            gsl_vector_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a698d2cd6e4da73ca0b4831e09f9c4419">sigmaPoint</a>);</div>
<div class="line"><a name="l00120"></a><span class="lineno">  120</span>&#160;</div>
<div class="line"><a name="l00121"></a><span class="lineno">  121</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a458721ed2b8869be17e7589024c6bac3">sigmaPoints</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a2a382b59ee2aefa913f3bc24dc70bb44">nbSamples</a>);</div>
<div class="line"><a name="l00122"></a><span class="lineno">  122</span>&#160;            gsl_matrix_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a458721ed2b8869be17e7589024c6bac3">sigmaPoints</a>);</div>
<div class="line"><a name="l00123"></a><span class="lineno">  123</span>&#160;</div>
<div class="line"><a name="l00124"></a><span class="lineno">  124</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#afea69f7b0d410986ff5f5ca92d70545e">sigmaPointMeasure</a> = gsl_vector_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>);</div>
<div class="line"><a name="l00125"></a><span class="lineno">  125</span>&#160;            gsl_vector_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a698d2cd6e4da73ca0b4831e09f9c4419">sigmaPoint</a>);</div>
<div class="line"><a name="l00126"></a><span class="lineno">  126</span>&#160;</div>
<div class="line"><a name="l00127"></a><span class="lineno">  127</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a90056d939d833550de52374098d3e348">sigmaPointsMeasure</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a45062e1fd58806a27f0e4f75772e33d2">nbSamplesMeasure</a>);</div>
<div class="line"><a name="l00128"></a><span class="lineno">  128</span>&#160;            gsl_matrix_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a90056d939d833550de52374098d3e348">sigmaPointsMeasure</a>);</div>
<div class="line"><a name="l00129"></a><span class="lineno">  129</span>&#160;</div>
<div class="line"><a name="l00130"></a><span class="lineno">  130</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a903d42d54de210cc63826a6578d64b3c">U</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>);</div>
<div class="line"><a name="l00131"></a><span class="lineno">  131</span>&#160;            gsl_matrix_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a903d42d54de210cc63826a6578d64b3c">U</a>);</div>
<div class="line"><a name="l00132"></a><span class="lineno">  132</span>&#160;</div>
<div class="line"><a name="l00133"></a><span class="lineno">  133</span>&#160;            <span class="comment">// Weights used to update the statistics</span></div>
<div class="line"><a name="l00134"></a><span class="lineno">  134</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0ab8c07796764941502c6a06f7d5b4dc">wm_j</a>  = gsl_vector_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a2a382b59ee2aefa913f3bc24dc70bb44">nbSamples</a>); <span class="comment">// Weights used to compute the mean of the sigma points images</span></div>
<div class="line"><a name="l00135"></a><span class="lineno">  135</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ae224406380a3ab3aff38a97a874c08f3">wc_j</a> = gsl_vector_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a2a382b59ee2aefa913f3bc24dc70bb44">nbSamples</a>); <span class="comment">// Weights used to update the covariance matrices</span></div>
<div class="line"><a name="l00136"></a><span class="lineno">  136</span>&#160;</div>
<div class="line"><a name="l00137"></a><span class="lineno">  137</span>&#160;            <span class="comment">// Set the weights</span></div>
<div class="line"><a name="l00138"></a><span class="lineno">  138</span>&#160;            gsl_vector_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0ab8c07796764941502c6a06f7d5b4dc">wm_j</a>, 0, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#aedb04b04c2a74208f787e26eb3f406f5">lambda</a> / (p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#aedb04b04c2a74208f787e26eb3f406f5">lambda</a>));</div>
<div class="line"><a name="l00139"></a><span class="lineno">  139</span>&#160;            gsl_vector_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ae224406380a3ab3aff38a97a874c08f3">wc_j</a>, 0, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#aedb04b04c2a74208f787e26eb3f406f5">lambda</a> / (p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#aedb04b04c2a74208f787e26eb3f406f5">lambda</a>) + (1.0 - p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a55760e3aa010ff582f27067c6168972a">alpha</a>*p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a55760e3aa010ff582f27067c6168972a">alpha</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ab77dd9bb480fee611664a4fab007030a">beta</a>));</div>
<div class="line"><a name="l00140"></a><span class="lineno">  140</span>&#160;            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 1 ; j &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a2a382b59ee2aefa913f3bc24dc70bb44">nbSamples</a>; j ++)</div>
<div class="line"><a name="l00141"></a><span class="lineno">  141</span>&#160;            {</div>
<div class="line"><a name="l00142"></a><span class="lineno">  142</span>&#160;                gsl_vector_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0ab8c07796764941502c6a06f7d5b4dc">wm_j</a>, j, 1.0 / (2.0 * (p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#aedb04b04c2a74208f787e26eb3f406f5">lambda</a>)));</div>
<div class="line"><a name="l00143"></a><span class="lineno">  143</span>&#160;                gsl_vector_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ae224406380a3ab3aff38a97a874c08f3">wc_j</a>, j, 1.0 / (2.0 * (p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#aedb04b04c2a74208f787e26eb3f406f5">lambda</a>)));</div>
<div class="line"><a name="l00144"></a><span class="lineno">  144</span>&#160;            }</div>
<div class="line"><a name="l00145"></a><span class="lineno">  145</span>&#160;</div>
<div class="line"><a name="l00146"></a><span class="lineno">  146</span>&#160;            <span class="comment">// Set the weights</span></div>
<div class="line"><a name="l00147"></a><span class="lineno">  147</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ae5bd148e7b9dd43dca85f62058d5013f">wm_aug_j</a>  = gsl_vector_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a45062e1fd58806a27f0e4f75772e33d2">nbSamplesMeasure</a>); <span class="comment">// Weights used to compute the mean of the sigma points images</span></div>
<div class="line"><a name="l00148"></a><span class="lineno">  148</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a5cc587b3f1b1e905242a484df549e4a1">wc_aug_j</a> = gsl_vector_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a45062e1fd58806a27f0e4f75772e33d2">nbSamplesMeasure</a>); <span class="comment">// Weights used to update the covariance matrices</span></div>
<div class="line"><a name="l00149"></a><span class="lineno">  149</span>&#160;            gsl_vector_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ae5bd148e7b9dd43dca85f62058d5013f">wm_aug_j</a>, 0, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a4802c63baa1362da178bd2456a47704b">lambda_aug</a> / (2*p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a4802c63baa1362da178bd2456a47704b">lambda_aug</a>));</div>
<div class="line"><a name="l00150"></a><span class="lineno">  150</span>&#160;            gsl_vector_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a5cc587b3f1b1e905242a484df549e4a1">wc_aug_j</a>, 0, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a4802c63baa1362da178bd2456a47704b">lambda_aug</a> / (2*p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a4802c63baa1362da178bd2456a47704b">lambda_aug</a>) + (1.0 - p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a55760e3aa010ff582f27067c6168972a">alpha</a>*p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a55760e3aa010ff582f27067c6168972a">alpha</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ab77dd9bb480fee611664a4fab007030a">beta</a>));</div>
<div class="line"><a name="l00151"></a><span class="lineno">  151</span>&#160;            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> j = 1 ; j &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a45062e1fd58806a27f0e4f75772e33d2">nbSamplesMeasure</a>; j ++)</div>
<div class="line"><a name="l00152"></a><span class="lineno">  152</span>&#160;            {</div>
<div class="line"><a name="l00153"></a><span class="lineno">  153</span>&#160;                gsl_vector_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ae5bd148e7b9dd43dca85f62058d5013f">wm_aug_j</a>, j, 1.0 / (2.0 * (2*p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a4802c63baa1362da178bd2456a47704b">lambda_aug</a>)));</div>
<div class="line"><a name="l00154"></a><span class="lineno">  154</span>&#160;                gsl_vector_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a5cc587b3f1b1e905242a484df549e4a1">wc_aug_j</a>, j, 1.0 / (2.0 * (2*p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a4802c63baa1362da178bd2456a47704b">lambda_aug</a>)));</div>
<div class="line"><a name="l00155"></a><span class="lineno">  155</span>&#160;            }</div>
<div class="line"><a name="l00156"></a><span class="lineno">  156</span>&#160;</div>
<div class="line"><a name="l00157"></a><span class="lineno">  157</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a43bbdfbb7c743522ebc18b2e299c7c1e">Ki</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>);</div>
<div class="line"><a name="l00158"></a><span class="lineno">  158</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ab11872ef0fce0c3b1c02eb19e6198fb1">Ki_T</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>);</div>
<div class="line"><a name="l00159"></a><span class="lineno">  159</span>&#160;</div>
<div class="line"><a name="l00160"></a><span class="lineno">  160</span>&#160;            <span class="comment">// Allocate temporary matrices</span></div>
<div class="line"><a name="l00161"></a><span class="lineno">  161</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a2158f781908727006d11975d94c55ad0">temp_n</a> = gsl_vector_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>);</div>
<div class="line"><a name="l00162"></a><span class="lineno">  162</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a64e81fa0a22add1316bd66d03e70f105">temp_no</a> = gsl_vector_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>);</div>
<div class="line"><a name="l00163"></a><span class="lineno">  163</span>&#160;</div>
<div class="line"><a name="l00164"></a><span class="lineno">  164</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a74e1d652fbafe8fe531bd6c61ef33ed8">temp_n_1</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>,1);</div>
<div class="line"><a name="l00165"></a><span class="lineno">  165</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a231b41faf198e81926477756e3f2c2e5">temp_1_n</a> = gsl_matrix_alloc(1,p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>);</div>
<div class="line"><a name="l00166"></a><span class="lineno">  166</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0cd68e2eff4ab4feed22bcdb8ffcb9b8">temp_n_n</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>);</div>
<div class="line"><a name="l00167"></a><span class="lineno">  167</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a483422efe8d15e91ff527261fbfbbbb7">temp_n_no</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>);</div>
<div class="line"><a name="l00168"></a><span class="lineno">  168</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a051b7e63dd546f62f0c57df7fac63efb">temp_no_1</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>,1);</div>
<div class="line"><a name="l00169"></a><span class="lineno">  169</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a00ab5d6e7673817ab87c143df0e3b698">temp_1_no</a> = gsl_matrix_alloc(1,p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>);</div>
<div class="line"><a name="l00170"></a><span class="lineno">  170</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a961545fff86f5f611e5a64f76166be0c">temp_no_no</a> = gsl_matrix_alloc(p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>);</div>
<div class="line"><a name="l00171"></a><span class="lineno">  171</span>&#160;</div>
<div class="line"><a name="l00172"></a><span class="lineno">  172</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a478e32bd9fdb16834d784f6a30954510">temp_3n_n</a> = gsl_matrix_alloc(3*p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>);</div>
<div class="line"><a name="l00173"></a><span class="lineno">  173</span>&#160;            s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#abaf7742e033e303907f9084a89f22301">temp_2nno_no</a> = gsl_matrix_alloc(2*p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>);</div>
<div class="line"><a name="l00174"></a><span class="lineno">  174</span>&#160;        }</div>
<div class="line"><a name="l00175"></a><span class="lineno">  175</span>&#160;</div>
<div class="line"><a name="l00180"></a><span class="lineno"><a class="line" href="namespaceukf_1_1srstate.html#a3a2cccd808cea8666029bcbfc44ba460">  180</a></span>&#160;        <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="namespaceukf_1_1srstate.html#a3a2cccd808cea8666029bcbfc44ba460">ukf_free</a>(<a class="code" href="structukf_1_1srstate_1_1ukf__param.html">ukf_param</a> &amp;p, <a class="code" href="structukf_1_1srstate_1_1ukf__state.html">ukf_state</a> &amp;s)</div>
<div class="line"><a name="l00181"></a><span class="lineno">  181</span>&#160;        {</div>
<div class="line"><a name="l00182"></a><span class="lineno">  182</span>&#160;            gsl_vector_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a9095ffca4f0bc5cbf7a42368cf7463d6">xi</a>);</div>
<div class="line"><a name="l00183"></a><span class="lineno">  183</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0876b435bf2ca99901c779bde40d9004">xi_prediction</a>);</div>
<div class="line"><a name="l00184"></a><span class="lineno">  184</span>&#160;            gsl_vector_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0ead044a78987bebf3a2df35efdcead3">xi_mean</a>);</div>
<div class="line"><a name="l00185"></a><span class="lineno">  185</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">Sxi</a>);</div>
<div class="line"><a name="l00186"></a><span class="lineno">  186</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a51c541f3e8ba1379cb5f61876e17c375">Svi</a>);</div>
<div class="line"><a name="l00187"></a><span class="lineno">  187</span>&#160;</div>
<div class="line"><a name="l00188"></a><span class="lineno">  188</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ac205dee75be43b32c76b0696d0182ab6">yi_prediction</a>);</div>
<div class="line"><a name="l00189"></a><span class="lineno">  189</span>&#160;            gsl_vector_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#af0b49a1284aa6ce02d8fe249df41d434">yi_mean</a>);</div>
<div class="line"><a name="l00190"></a><span class="lineno">  190</span>&#160;            gsl_vector_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#add7a90f3736c8909a2e3abc3e4235ba7">ino_yi</a>);</div>
<div class="line"><a name="l00191"></a><span class="lineno">  191</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a70875ec1b32a4a73816c139ebe54d17e">Syi</a>);</div>
<div class="line"><a name="l00192"></a><span class="lineno">  192</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#afee48e0e83f20d7d46a478a9b436a42b">Sni</a>);</div>
<div class="line"><a name="l00193"></a><span class="lineno">  193</span>&#160;</div>
<div class="line"><a name="l00194"></a><span class="lineno">  194</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0d210360e59328e219b5f37751baac35">Pxyi</a>);</div>
<div class="line"><a name="l00195"></a><span class="lineno">  195</span>&#160;</div>
<div class="line"><a name="l00196"></a><span class="lineno">  196</span>&#160;            gsl_vector_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a698d2cd6e4da73ca0b4831e09f9c4419">sigmaPoint</a>);</div>
<div class="line"><a name="l00197"></a><span class="lineno">  197</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a458721ed2b8869be17e7589024c6bac3">sigmaPoints</a>);</div>
<div class="line"><a name="l00198"></a><span class="lineno">  198</span>&#160;</div>
<div class="line"><a name="l00199"></a><span class="lineno">  199</span>&#160;            gsl_vector_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#afea69f7b0d410986ff5f5ca92d70545e">sigmaPointMeasure</a>);</div>
<div class="line"><a name="l00200"></a><span class="lineno">  200</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a90056d939d833550de52374098d3e348">sigmaPointsMeasure</a>);</div>
<div class="line"><a name="l00201"></a><span class="lineno">  201</span>&#160;</div>
<div class="line"><a name="l00202"></a><span class="lineno">  202</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a903d42d54de210cc63826a6578d64b3c">U</a>);</div>
<div class="line"><a name="l00203"></a><span class="lineno">  203</span>&#160;</div>
<div class="line"><a name="l00204"></a><span class="lineno">  204</span>&#160;            gsl_vector_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0ab8c07796764941502c6a06f7d5b4dc">wm_j</a>);</div>
<div class="line"><a name="l00205"></a><span class="lineno">  205</span>&#160;            gsl_vector_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ae224406380a3ab3aff38a97a874c08f3">wc_j</a>);</div>
<div class="line"><a name="l00206"></a><span class="lineno">  206</span>&#160;</div>
<div class="line"><a name="l00207"></a><span class="lineno">  207</span>&#160;            gsl_vector_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ae5bd148e7b9dd43dca85f62058d5013f">wm_aug_j</a>);</div>
<div class="line"><a name="l00208"></a><span class="lineno">  208</span>&#160;            gsl_vector_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a5cc587b3f1b1e905242a484df549e4a1">wc_aug_j</a>);</div>
<div class="line"><a name="l00209"></a><span class="lineno">  209</span>&#160;</div>
<div class="line"><a name="l00210"></a><span class="lineno">  210</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a43bbdfbb7c743522ebc18b2e299c7c1e">Ki</a>);</div>
<div class="line"><a name="l00211"></a><span class="lineno">  211</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ab11872ef0fce0c3b1c02eb19e6198fb1">Ki_T</a>);</div>
<div class="line"><a name="l00212"></a><span class="lineno">  212</span>&#160;</div>
<div class="line"><a name="l00213"></a><span class="lineno">  213</span>&#160;            gsl_vector_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a2158f781908727006d11975d94c55ad0">temp_n</a>);</div>
<div class="line"><a name="l00214"></a><span class="lineno">  214</span>&#160;            gsl_vector_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a64e81fa0a22add1316bd66d03e70f105">temp_no</a>);</div>
<div class="line"><a name="l00215"></a><span class="lineno">  215</span>&#160;</div>
<div class="line"><a name="l00216"></a><span class="lineno">  216</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a74e1d652fbafe8fe531bd6c61ef33ed8">temp_n_1</a>);</div>
<div class="line"><a name="l00217"></a><span class="lineno">  217</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a231b41faf198e81926477756e3f2c2e5">temp_1_n</a>);</div>
<div class="line"><a name="l00218"></a><span class="lineno">  218</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0cd68e2eff4ab4feed22bcdb8ffcb9b8">temp_n_n</a>);</div>
<div class="line"><a name="l00219"></a><span class="lineno">  219</span>&#160;</div>
<div class="line"><a name="l00220"></a><span class="lineno">  220</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a483422efe8d15e91ff527261fbfbbbb7">temp_n_no</a>);</div>
<div class="line"><a name="l00221"></a><span class="lineno">  221</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a051b7e63dd546f62f0c57df7fac63efb">temp_no_1</a>);</div>
<div class="line"><a name="l00222"></a><span class="lineno">  222</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a00ab5d6e7673817ab87c143df0e3b698">temp_1_no</a>);</div>
<div class="line"><a name="l00223"></a><span class="lineno">  223</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a961545fff86f5f611e5a64f76166be0c">temp_no_no</a>);</div>
<div class="line"><a name="l00224"></a><span class="lineno">  224</span>&#160;</div>
<div class="line"><a name="l00225"></a><span class="lineno">  225</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a478e32bd9fdb16834d784f6a30954510">temp_3n_n</a>);</div>
<div class="line"><a name="l00226"></a><span class="lineno">  226</span>&#160;            gsl_matrix_free(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#abaf7742e033e303907f9084a89f22301">temp_2nno_no</a>);</div>
<div class="line"><a name="l00227"></a><span class="lineno">  227</span>&#160;        }</div>
<div class="line"><a name="l00228"></a><span class="lineno">  228</span>&#160;</div>
<div class="line"><a name="l00233"></a><span class="lineno">  233</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> FunctProcess, <span class="keyword">typename</span> FunctObservation&gt;</div>
<div class="line"><a name="l00234"></a><span class="lineno"><a class="line" href="namespaceukf_1_1srstate.html#ada14e67760dda9e7a06a957a0a52a983">  234</a></span>&#160;                <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="namespaceukf_1_1srstate.html#ada14e67760dda9e7a06a957a0a52a983">ukf_iterate</a>(<a class="code" href="structukf_1_1srstate_1_1ukf__param.html">ukf_param</a> &amp;p, <a class="code" href="structukf_1_1srstate_1_1ukf__state.html">ukf_state</a> &amp;s, FunctProcess f, FunctObservation h, gsl_vector* yi)</div>
<div class="line"><a name="l00235"></a><span class="lineno">  235</span>&#160;        {</div>
<div class="line"><a name="l00236"></a><span class="lineno">  236</span>&#160;            <span class="keywordtype">int</span> i,j;</div>
<div class="line"><a name="l00237"></a><span class="lineno">  237</span>&#160;            gsl_matrix_view mat_view;</div>
<div class="line"><a name="l00238"></a><span class="lineno">  238</span>&#160;            gsl_vector_view vec_view;</div>
<div class="line"><a name="l00239"></a><span class="lineno">  239</span>&#160;            <span class="comment">// ************************************************** //</span></div>
<div class="line"><a name="l00240"></a><span class="lineno">  240</span>&#160;            <span class="comment">// ************ Compute the sigma points ************ //</span></div>
<div class="line"><a name="l00241"></a><span class="lineno">  241</span>&#160;            <span class="comment">// ************************************************** //</span></div>
<div class="line"><a name="l00242"></a><span class="lineno">  242</span>&#160;</div>
<div class="line"><a name="l00243"></a><span class="lineno">  243</span>&#160;            <span class="comment">// 1- Compute the sigma points,</span></div>
<div class="line"><a name="l00244"></a><span class="lineno">  244</span>&#160;            <span class="comment">// Equation (3.209)</span></div>
<div class="line"><a name="l00245"></a><span class="lineno">  245</span>&#160;            <span class="comment">// sigmapoint_j = x_(i-1)</span></div>
<div class="line"><a name="l00246"></a><span class="lineno">  246</span>&#160;            <span class="comment">// sigmapoint_j = x_(i-1) + gamma * Sxi_j for 1 &lt;= j &lt;= n</span></div>
<div class="line"><a name="l00247"></a><span class="lineno">  247</span>&#160;            <span class="comment">// sigmapoint_j = x_(i-1) - gamma * Sxi_(j-(n+1)) for n+1 &lt;= j &lt;= 2n</span></div>
<div class="line"><a name="l00248"></a><span class="lineno">  248</span>&#160;            gsl_matrix_set_col(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a458721ed2b8869be17e7589024c6bac3">sigmaPoints</a>, 0, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a9095ffca4f0bc5cbf7a42368cf7463d6">xi</a>);</div>
<div class="line"><a name="l00249"></a><span class="lineno">  249</span>&#160;            <span class="keywordflow">for</span>(j = 1 ; j &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> + 1 ; ++j)</div>
<div class="line"><a name="l00250"></a><span class="lineno">  250</span>&#160;                <span class="keywordflow">for</span>(i = 0 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> ; ++i)</div>
<div class="line"><a name="l00251"></a><span class="lineno">  251</span>&#160;                {</div>
<div class="line"><a name="l00252"></a><span class="lineno">  252</span>&#160;                gsl_matrix_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a458721ed2b8869be17e7589024c6bac3">sigmaPoints</a>,i,j, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a9095ffca4f0bc5cbf7a42368cf7463d6">xi</a>-&gt;data[i] + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a5dc27e0d7ae329c6456efdeb0c359a39">gamma</a> * gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">Sxi</a>, i, j-1));</div>
<div class="line"><a name="l00253"></a><span class="lineno">  253</span>&#160;                gsl_matrix_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a458721ed2b8869be17e7589024c6bac3">sigmaPoints</a>,i,j+p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a9095ffca4f0bc5cbf7a42368cf7463d6">xi</a>-&gt;data[i] - p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a5dc27e0d7ae329c6456efdeb0c359a39">gamma</a> * gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">Sxi</a>, i, j-1));</div>
<div class="line"><a name="l00254"></a><span class="lineno">  254</span>&#160;            }</div>
<div class="line"><a name="l00255"></a><span class="lineno">  255</span>&#160;</div>
<div class="line"><a name="l00256"></a><span class="lineno">  256</span>&#160;            <span class="comment">/**********************************/</span></div>
<div class="line"><a name="l00257"></a><span class="lineno">  257</span>&#160;            <span class="comment">/***** Time update equations  *****/</span></div>
<div class="line"><a name="l00258"></a><span class="lineno">  258</span>&#160;            <span class="comment">/**********************************/</span></div>
<div class="line"><a name="l00259"></a><span class="lineno">  259</span>&#160;</div>
<div class="line"><a name="l00260"></a><span class="lineno">  260</span>&#160;            <span class="comment">// Time update equations</span></div>
<div class="line"><a name="l00261"></a><span class="lineno">  261</span>&#160;            <span class="comment">// 0 - Compute the image of the sigma points and the mean of these images</span></div>
<div class="line"><a name="l00262"></a><span class="lineno">  262</span>&#160;            gsl_vector_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0ead044a78987bebf3a2df35efdcead3">xi_mean</a>);</div>
<div class="line"><a name="l00263"></a><span class="lineno">  263</span>&#160;            <span class="keywordflow">for</span>(j = 0 ; j &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a2a382b59ee2aefa913f3bc24dc70bb44">nbSamples</a> ; ++j)</div>
<div class="line"><a name="l00264"></a><span class="lineno">  264</span>&#160;            {</div>
<div class="line"><a name="l00265"></a><span class="lineno">  265</span>&#160;                gsl_matrix_get_col(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a698d2cd6e4da73ca0b4831e09f9c4419">sigmaPoint</a>, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a458721ed2b8869be17e7589024c6bac3">sigmaPoints</a>, j);</div>
<div class="line"><a name="l00266"></a><span class="lineno">  266</span>&#160;                <span class="comment">// Compute the image of the sigma points</span></div>
<div class="line"><a name="l00267"></a><span class="lineno">  267</span>&#160;                <span class="comment">// Eq 3.210</span></div>
<div class="line"><a name="l00268"></a><span class="lineno">  268</span>&#160;                vec_view = gsl_matrix_column(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0876b435bf2ca99901c779bde40d9004">xi_prediction</a>,j);</div>
<div class="line"><a name="l00269"></a><span class="lineno">  269</span>&#160;                f(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a923a0facfe402f098a4da50e8f184f61">params</a>, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a698d2cd6e4da73ca0b4831e09f9c4419">sigmaPoint</a>, &amp;vec_view.vector);</div>
<div class="line"><a name="l00270"></a><span class="lineno">  270</span>&#160;</div>
<div class="line"><a name="l00271"></a><span class="lineno">  271</span>&#160;                <span class="comment">// Update the mean, Eq (3.211)</span></div>
<div class="line"><a name="l00272"></a><span class="lineno">  272</span>&#160;                <span class="keywordflow">for</span>(i = 0 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> ; ++i)</div>
<div class="line"><a name="l00273"></a><span class="lineno">  273</span>&#160;                    s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0ead044a78987bebf3a2df35efdcead3">xi_mean</a>-&gt;data[i] += s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0ab8c07796764941502c6a06f7d5b4dc">wm_j</a>-&gt;data[j] * gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0876b435bf2ca99901c779bde40d9004">xi_prediction</a>,i,j);</div>
<div class="line"><a name="l00274"></a><span class="lineno">  274</span>&#160;            }</div>
<div class="line"><a name="l00275"></a><span class="lineno">  275</span>&#160;</div>
<div class="line"><a name="l00276"></a><span class="lineno">  276</span>&#160;            <span class="comment">// 1- Compute the QR decomposition and keeps only the upper triangular part of R</span></div>
<div class="line"><a name="l00277"></a><span class="lineno">  277</span>&#160;            <span class="comment">// Eq. 3.212</span></div>
<div class="line"><a name="l00278"></a><span class="lineno">  278</span>&#160;            <span class="comment">// We first fill the matrix Sxk^-</span></div>
<div class="line"><a name="l00279"></a><span class="lineno">  279</span>&#160;            <span class="comment">// with the transpose of what is in the brackets of qr { ... }</span></div>
<div class="line"><a name="l00280"></a><span class="lineno">  280</span>&#160;            <span class="keywordflow">for</span>(j = 1 ; j &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a2a382b59ee2aefa913f3bc24dc70bb44">nbSamples</a>; ++j)</div>
<div class="line"><a name="l00281"></a><span class="lineno">  281</span>&#160;            {</div>
<div class="line"><a name="l00282"></a><span class="lineno">  282</span>&#160;                <span class="keywordflow">for</span>(i = 0 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> ; ++i)</div>
<div class="line"><a name="l00283"></a><span class="lineno">  283</span>&#160;                {</div>
<div class="line"><a name="l00284"></a><span class="lineno">  284</span>&#160;                    gsl_matrix_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a478e32bd9fdb16834d784f6a30954510">temp_3n_n</a>, j-1, i, sqrt(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ae224406380a3ab3aff38a97a874c08f3">wc_j</a>-&gt;data[1]) * (gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0876b435bf2ca99901c779bde40d9004">xi_prediction</a>,i,j) - s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0ead044a78987bebf3a2df35efdcead3">xi_mean</a>-&gt;data[i]));</div>
<div class="line"><a name="l00285"></a><span class="lineno">  285</span>&#160;                }</div>
<div class="line"><a name="l00286"></a><span class="lineno">  286</span>&#160;            }</div>
<div class="line"><a name="l00287"></a><span class="lineno">  287</span>&#160;            <span class="comment">// The last rows are filled with the cholesky factor of the process noise covariance</span></div>
<div class="line"><a name="l00288"></a><span class="lineno">  288</span>&#160;            <span class="keywordflow">for</span>(j = 0 ; j &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> ; ++j)</div>
<div class="line"><a name="l00289"></a><span class="lineno">  289</span>&#160;            {</div>
<div class="line"><a name="l00290"></a><span class="lineno">  290</span>&#160;              mat_view = gsl_matrix_submatrix(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a478e32bd9fdb16834d784f6a30954510">temp_3n_n</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a2a382b59ee2aefa913f3bc24dc70bb44">nbSamples</a>-1, 0, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>);</div>
<div class="line"><a name="l00291"></a><span class="lineno">  291</span>&#160;                gsl_matrix_memcpy(&amp;mat_view.matrix, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a51c541f3e8ba1379cb5f61876e17c375">Svi</a>);</div>
<div class="line"><a name="l00292"></a><span class="lineno">  292</span>&#160;            }</div>
<div class="line"><a name="l00293"></a><span class="lineno">  293</span>&#160;            <span class="comment">// We now perform the QR decomposition of s.temp_3n_n;</span></div>
<div class="line"><a name="l00294"></a><span class="lineno">  294</span>&#160;            gsl_linalg_QR_decomp(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a478e32bd9fdb16834d784f6a30954510">temp_3n_n</a>, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a2158f781908727006d11975d94c55ad0">temp_n</a>);</div>
<div class="line"><a name="l00295"></a><span class="lineno">  295</span>&#160;            <span class="comment">// From this QR decomposition, we keep only the upper triangular part of R and copy it in s.Sxi</span></div>
<div class="line"><a name="l00296"></a><span class="lineno">  296</span>&#160;            <span class="keywordflow">for</span>( j = 0 ; j &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> ; ++j)</div>
<div class="line"><a name="l00297"></a><span class="lineno">  297</span>&#160;            {</div>
<div class="line"><a name="l00298"></a><span class="lineno">  298</span>&#160;                <span class="keywordflow">for</span>(i = 0 ; i &lt; j+1 ; ++i)</div>
<div class="line"><a name="l00299"></a><span class="lineno">  299</span>&#160;                    gsl_matrix_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">Sxi</a>, i, j, gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a478e32bd9fdb16834d784f6a30954510">temp_3n_n</a>, i, j));</div>
<div class="line"><a name="l00300"></a><span class="lineno">  300</span>&#160;</div>
<div class="line"><a name="l00301"></a><span class="lineno">  301</span>&#160;                <span class="keywordflow">for</span>(i = j+1 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> ; ++i)</div>
<div class="line"><a name="l00302"></a><span class="lineno">  302</span>&#160;                    gsl_matrix_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">Sxi</a>, i, j, 0.0);</div>
<div class="line"><a name="l00303"></a><span class="lineno">  303</span>&#160;            }</div>
<div class="line"><a name="l00304"></a><span class="lineno">  304</span>&#160;            <span class="comment">//gsl_matrix_transpose(s.Sxi);</span></div>
<div class="line"><a name="l00305"></a><span class="lineno">  305</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) <a class="code" href="ukf__sr__state__ndim_8h.html#a2b6b76843e5a380b93a5cd4238a688a8">print_mat</a>(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">Sxi</a>);</div>
<div class="line"><a name="l00306"></a><span class="lineno">  306</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) printf(<span class="stringliteral">&quot;\n&quot;</span>);</div>
<div class="line"><a name="l00307"></a><span class="lineno">  307</span>&#160;</div>
<div class="line"><a name="l00308"></a><span class="lineno">  308</span>&#160;            <span class="comment">// 2- Perform the cholupdate, which is a rank one update</span></div>
<div class="line"><a name="l00309"></a><span class="lineno">  309</span>&#160;            <span class="comment">// Eq. 3.213</span></div>
<div class="line"><a name="l00310"></a><span class="lineno">  310</span>&#160;            <span class="keywordflow">for</span>(i = 0 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> ; ++i)</div>
<div class="line"><a name="l00311"></a><span class="lineno">  311</span>&#160;                s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a2158f781908727006d11975d94c55ad0">temp_n</a>-&gt;data[i] = gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0876b435bf2ca99901c779bde40d9004">xi_prediction</a>,i,0) - s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0ead044a78987bebf3a2df35efdcead3">xi_mean</a>-&gt;data[i];</div>
<div class="line"><a name="l00312"></a><span class="lineno">  312</span>&#160;</div>
<div class="line"><a name="l00313"></a><span class="lineno">  313</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) printf(<span class="stringliteral">&quot;Before cholupdate 1 \n&quot;</span>);</div>
<div class="line"><a name="l00314"></a><span class="lineno">  314</span>&#160;            gsl_matrix_transpose(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">Sxi</a>);</div>
<div class="line"><a name="l00315"></a><span class="lineno">  315</span>&#160;            <a class="code" href="namespaceukf_1_1math.html#ac921eed0b4618cb2e860cf712c676675">ukf::math::choleskyUpdate</a>(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">Sxi</a>, <a class="code" href="namespaceukf_1_1math.html#a8123c498310baa12b0985a3bd271c06c">ukf::math::signof</a>(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ae224406380a3ab3aff38a97a874c08f3">wc_j</a>-&gt;data[0]) * sqrt(fabs(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ae224406380a3ab3aff38a97a874c08f3">wc_j</a>-&gt;data[0])), s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a2158f781908727006d11975d94c55ad0">temp_n</a>);</div>
<div class="line"><a name="l00316"></a><span class="lineno">  316</span>&#160;            <span class="comment">// Warning : Here, we keep Sxi as a triangular inferior matrix</span></div>
<div class="line"><a name="l00317"></a><span class="lineno">  317</span>&#160;</div>
<div class="line"><a name="l00318"></a><span class="lineno">  318</span>&#160;            <span class="comment">//gsl_matrix_transpose(s.Sxi);</span></div>
<div class="line"><a name="l00319"></a><span class="lineno">  319</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) printf(<span class="stringliteral">&quot;ok\n&quot;</span>);</div>
<div class="line"><a name="l00320"></a><span class="lineno">  320</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) printf(<span class="stringliteral">&quot;Sxi after cholupdate : \n&quot;</span>);</div>
<div class="line"><a name="l00321"></a><span class="lineno">  321</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) <a class="code" href="ukf__sr__state__ndim_8h.html#a2b6b76843e5a380b93a5cd4238a688a8">print_mat</a>(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">Sxi</a>);</div>
<div class="line"><a name="l00322"></a><span class="lineno">  322</span>&#160;            <span class="comment">// ** Augment the sigma points ** //</span></div>
<div class="line"><a name="l00323"></a><span class="lineno">  323</span>&#160;            <span class="comment">// Eq 3.124</span></div>
<div class="line"><a name="l00324"></a><span class="lineno">  324</span>&#160;</div>
<div class="line"><a name="l00325"></a><span class="lineno">  325</span>&#160;            <span class="comment">// 1 - Copy the images of the original sigma points</span></div>
<div class="line"><a name="l00326"></a><span class="lineno">  326</span>&#160;            mat_view = gsl_matrix_submatrix(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a90056d939d833550de52374098d3e348">sigmaPointsMeasure</a>,0,0, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a2a382b59ee2aefa913f3bc24dc70bb44">nbSamples</a>);</div>
<div class="line"><a name="l00327"></a><span class="lineno">  327</span>&#160;            gsl_matrix_memcpy(&amp;mat_view.matrix, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0876b435bf2ca99901c779bde40d9004">xi_prediction</a>);</div>
<div class="line"><a name="l00328"></a><span class="lineno">  328</span>&#160;</div>
<div class="line"><a name="l00329"></a><span class="lineno">  329</span>&#160;            <span class="comment">// 2 - Compute the additional sigma points</span></div>
<div class="line"><a name="l00330"></a><span class="lineno">  330</span>&#160;            <span class="keywordflow">for</span>(j = 0 ; j &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> ; ++j)</div>
<div class="line"><a name="l00331"></a><span class="lineno">  331</span>&#160;            {</div>
<div class="line"><a name="l00332"></a><span class="lineno">  332</span>&#160;                <span class="keywordflow">for</span>(i = 0 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> ; ++i)</div>
<div class="line"><a name="l00333"></a><span class="lineno">  333</span>&#160;                {</div>
<div class="line"><a name="l00334"></a><span class="lineno">  334</span>&#160;                    gsl_matrix_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a90056d939d833550de52374098d3e348">sigmaPointsMeasure</a>, i ,j + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a2a382b59ee2aefa913f3bc24dc70bb44">nbSamples</a> , gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0876b435bf2ca99901c779bde40d9004">xi_prediction</a>, i, 0) + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#aea53a3550db37bc15a3556df4c4edad8">gamma_aug</a> * gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a51c541f3e8ba1379cb5f61876e17c375">Svi</a>, i,j));</div>
<div class="line"><a name="l00335"></a><span class="lineno">  335</span>&#160;                    gsl_matrix_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a90056d939d833550de52374098d3e348">sigmaPointsMeasure</a>, i ,j + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a2a382b59ee2aefa913f3bc24dc70bb44">nbSamples</a> + p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> , gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0876b435bf2ca99901c779bde40d9004">xi_prediction</a>, i, 0) - p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#aea53a3550db37bc15a3556df4c4edad8">gamma_aug</a> * gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a51c541f3e8ba1379cb5f61876e17c375">Svi</a>, i,j));</div>
<div class="line"><a name="l00336"></a><span class="lineno">  336</span>&#160;                }</div>
<div class="line"><a name="l00337"></a><span class="lineno">  337</span>&#160;            }</div>
<div class="line"><a name="l00338"></a><span class="lineno">  338</span>&#160;</div>
<div class="line"><a name="l00339"></a><span class="lineno">  339</span>&#160;            <span class="comment">// Compute the image of the sigma points, i.e. the associated predicted observations</span></div>
<div class="line"><a name="l00340"></a><span class="lineno">  340</span>&#160;            <span class="comment">// and their mean</span></div>
<div class="line"><a name="l00341"></a><span class="lineno">  341</span>&#160;            gsl_vector_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#af0b49a1284aa6ce02d8fe249df41d434">yi_mean</a>);</div>
<div class="line"><a name="l00342"></a><span class="lineno">  342</span>&#160;            <span class="keywordflow">for</span>(j = 0 ; j &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a45062e1fd58806a27f0e4f75772e33d2">nbSamplesMeasure</a> ; ++j)</div>
<div class="line"><a name="l00343"></a><span class="lineno">  343</span>&#160;            {</div>
<div class="line"><a name="l00344"></a><span class="lineno">  344</span>&#160;                gsl_matrix_get_col(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#afea69f7b0d410986ff5f5ca92d70545e">sigmaPointMeasure</a>, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a90056d939d833550de52374098d3e348">sigmaPointsMeasure</a>, j);</div>
<div class="line"><a name="l00345"></a><span class="lineno">  345</span>&#160;                vec_view = gsl_matrix_column(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ac205dee75be43b32c76b0696d0182ab6">yi_prediction</a>,j);</div>
<div class="line"><a name="l00346"></a><span class="lineno">  346</span>&#160;                h(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#afea69f7b0d410986ff5f5ca92d70545e">sigmaPointMeasure</a>, &amp;vec_view.vector);</div>
<div class="line"><a name="l00347"></a><span class="lineno">  347</span>&#160;</div>
<div class="line"><a name="l00348"></a><span class="lineno">  348</span>&#160;                <span class="comment">// Update the mean , eq (3.176)</span></div>
<div class="line"><a name="l00349"></a><span class="lineno">  349</span>&#160;                <span class="keywordflow">for</span>(i = 0 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a> ; ++i)</div>
<div class="line"><a name="l00350"></a><span class="lineno">  350</span>&#160;                    s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#af0b49a1284aa6ce02d8fe249df41d434">yi_mean</a>-&gt;data[i] += s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ae5bd148e7b9dd43dca85f62058d5013f">wm_aug_j</a>-&gt;data[j] * gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ac205dee75be43b32c76b0696d0182ab6">yi_prediction</a>,i,j);</div>
<div class="line"><a name="l00351"></a><span class="lineno">  351</span>&#160;            }</div>
<div class="line"><a name="l00352"></a><span class="lineno">  352</span>&#160;</div>
<div class="line"><a name="l00353"></a><span class="lineno">  353</span>&#160;            <span class="comment">/*****************************************/</span></div>
<div class="line"><a name="l00354"></a><span class="lineno">  354</span>&#160;            <span class="comment">/***** Measurement update equations  *****/</span></div>
<div class="line"><a name="l00355"></a><span class="lineno">  355</span>&#160;            <span class="comment">/*****************************************/</span></div>
<div class="line"><a name="l00356"></a><span class="lineno">  356</span>&#160;</div>
<div class="line"><a name="l00357"></a><span class="lineno">  357</span>&#160;            <span class="comment">// Compute the Cholesky factor of the variance-covariance matrix of the observations</span></div>
<div class="line"><a name="l00358"></a><span class="lineno">  358</span>&#160;            <span class="comment">// Eq. 3.217</span></div>
<div class="line"><a name="l00359"></a><span class="lineno">  359</span>&#160;            <span class="comment">// First put in s.temp_2nno_no the transpose of what is in the brackets of qr{...} ,</span></div>
<div class="line"><a name="l00360"></a><span class="lineno">  360</span>&#160;            <span class="keywordflow">for</span>(j = 0 ; j &lt; 2*p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> ; ++j)</div>
<div class="line"><a name="l00361"></a><span class="lineno">  361</span>&#160;                <span class="keywordflow">for</span>(i = 0 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a> ; ++i)</div>
<div class="line"><a name="l00362"></a><span class="lineno">  362</span>&#160;                    gsl_matrix_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#abaf7742e033e303907f9084a89f22301">temp_2nno_no</a>, j, i, sqrt(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a5cc587b3f1b1e905242a484df549e4a1">wc_aug_j</a>-&gt;data[1])*(gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ac205dee75be43b32c76b0696d0182ab6">yi_prediction</a>, i, j+1) - s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#af0b49a1284aa6ce02d8fe249df41d434">yi_mean</a>-&gt;data[i]) );</div>
<div class="line"><a name="l00363"></a><span class="lineno">  363</span>&#160;</div>
<div class="line"><a name="l00364"></a><span class="lineno">  364</span>&#160;            <span class="comment">// Copy the Cholesky factor of the covariance of the observation noise</span></div>
<div class="line"><a name="l00365"></a><span class="lineno">  365</span>&#160;            mat_view = gsl_matrix_submatrix(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#abaf7742e033e303907f9084a89f22301">temp_2nno_no</a>,2*p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a>, 0, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>, p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a>);</div>
<div class="line"><a name="l00366"></a><span class="lineno">  366</span>&#160;            gsl_matrix_memcpy(&amp;mat_view.matrix, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#afee48e0e83f20d7d46a478a9b436a42b">Sni</a>);</div>
<div class="line"><a name="l00367"></a><span class="lineno">  367</span>&#160;</div>
<div class="line"><a name="l00368"></a><span class="lineno">  368</span>&#160;            <span class="comment">// We now perform the QR decomposition of s.temp_2nno_no;</span></div>
<div class="line"><a name="l00369"></a><span class="lineno">  369</span>&#160;            gsl_linalg_QR_decomp(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#abaf7742e033e303907f9084a89f22301">temp_2nno_no</a>, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a64e81fa0a22add1316bd66d03e70f105">temp_no</a>);</div>
<div class="line"><a name="l00370"></a><span class="lineno">  370</span>&#160;            <span class="comment">// From this QR decomposition, we keep only the upper triangular part of R and copy it in s.Syi</span></div>
<div class="line"><a name="l00371"></a><span class="lineno">  371</span>&#160;            <span class="keywordflow">for</span>( j = 0 ; j &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a> ; ++j)</div>
<div class="line"><a name="l00372"></a><span class="lineno">  372</span>&#160;            {</div>
<div class="line"><a name="l00373"></a><span class="lineno">  373</span>&#160;                <span class="keywordflow">for</span>(i = 0 ; i &lt; j+1 ; ++i)</div>
<div class="line"><a name="l00374"></a><span class="lineno">  374</span>&#160;                    gsl_matrix_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a70875ec1b32a4a73816c139ebe54d17e">Syi</a>, i, j, gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#abaf7742e033e303907f9084a89f22301">temp_2nno_no</a>, i, j));</div>
<div class="line"><a name="l00375"></a><span class="lineno">  375</span>&#160;</div>
<div class="line"><a name="l00376"></a><span class="lineno">  376</span>&#160;                <span class="keywordflow">for</span>(i = j+1 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a> ; ++i)</div>
<div class="line"><a name="l00377"></a><span class="lineno">  377</span>&#160;                    gsl_matrix_set(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a70875ec1b32a4a73816c139ebe54d17e">Syi</a>, i, j, 0.0);</div>
<div class="line"><a name="l00378"></a><span class="lineno">  378</span>&#160;            }</div>
<div class="line"><a name="l00379"></a><span class="lineno">  379</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) printf(<span class="stringliteral">&quot;Syi : \n&quot;</span>);</div>
<div class="line"><a name="l00380"></a><span class="lineno">  380</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) <a class="code" href="ukf__sr__state__ndim_8h.html#a2b6b76843e5a380b93a5cd4238a688a8">print_mat</a>(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a70875ec1b32a4a73816c139ebe54d17e">Syi</a>);</div>
<div class="line"><a name="l00381"></a><span class="lineno">  381</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) printf(<span class="stringliteral">&quot;\n&quot;</span>);</div>
<div class="line"><a name="l00382"></a><span class="lineno">  382</span>&#160;            <span class="comment">//gsl_matrix_transpose(s.Syi);</span></div>
<div class="line"><a name="l00383"></a><span class="lineno">  383</span>&#160;</div>
<div class="line"><a name="l00384"></a><span class="lineno">  384</span>&#160;            <span class="comment">// 2- Perform the cholupdate, which is a rank one update</span></div>
<div class="line"><a name="l00385"></a><span class="lineno">  385</span>&#160;            <span class="comment">// Eq. 3.218</span></div>
<div class="line"><a name="l00386"></a><span class="lineno">  386</span>&#160;            <span class="comment">// Syi is triangular superior</span></div>
<div class="line"><a name="l00387"></a><span class="lineno">  387</span>&#160;            <span class="keywordflow">for</span>(i = 0 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a> ; ++i)</div>
<div class="line"><a name="l00388"></a><span class="lineno">  388</span>&#160;                s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a64e81fa0a22add1316bd66d03e70f105">temp_no</a>-&gt;data[i] = gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ac205dee75be43b32c76b0696d0182ab6">yi_prediction</a>,i,0) - s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#af0b49a1284aa6ce02d8fe249df41d434">yi_mean</a>-&gt;data[i];</div>
<div class="line"><a name="l00389"></a><span class="lineno">  389</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) <a class="code" href="ukf__sr__state__ndim_8h.html#a7cfcd827e44f3bbf3fff23fbac44dfc8">print_vec</a>(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a64e81fa0a22add1316bd66d03e70f105">temp_no</a>);</div>
<div class="line"><a name="l00390"></a><span class="lineno">  390</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) printf(<span class="stringliteral">&quot;Before cholupdate 2 \n&quot;</span>);</div>
<div class="line"><a name="l00391"></a><span class="lineno">  391</span>&#160;            gsl_matrix_transpose(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a70875ec1b32a4a73816c139ebe54d17e">Syi</a>);</div>
<div class="line"><a name="l00392"></a><span class="lineno">  392</span>&#160;            <a class="code" href="namespaceukf_1_1math.html#ac921eed0b4618cb2e860cf712c676675">ukf::math::choleskyUpdate</a>(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a70875ec1b32a4a73816c139ebe54d17e">Syi</a>, <a class="code" href="namespaceukf_1_1math.html#a8123c498310baa12b0985a3bd271c06c">ukf::math::signof</a>(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a5cc587b3f1b1e905242a484df549e4a1">wc_aug_j</a>-&gt;data[0])*sqrt(fabs(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a5cc587b3f1b1e905242a484df549e4a1">wc_aug_j</a>-&gt;data[0])), s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a64e81fa0a22add1316bd66d03e70f105">temp_no</a>);</div>
<div class="line"><a name="l00393"></a><span class="lineno">  393</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) printf(<span class="stringliteral">&quot;ok \n&quot;</span>);</div>
<div class="line"><a name="l00394"></a><span class="lineno">  394</span>&#160;            <span class="comment">// Now, Syi is the lower triangular cholesky factor</span></div>
<div class="line"><a name="l00395"></a><span class="lineno">  395</span>&#160;            <span class="comment">//gsl_matrix_transpose(s.Syi);</span></div>
<div class="line"><a name="l00396"></a><span class="lineno">  396</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) printf(<span class="stringliteral">&quot;Syi after cholupdate : \n&quot;</span>);</div>
<div class="line"><a name="l00397"></a><span class="lineno">  397</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) <a class="code" href="ukf__sr__state__ndim_8h.html#a2b6b76843e5a380b93a5cd4238a688a8">print_mat</a>(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a70875ec1b32a4a73816c139ebe54d17e">Syi</a>);</div>
<div class="line"><a name="l00398"></a><span class="lineno">  398</span>&#160;</div>
<div class="line"><a name="l00399"></a><span class="lineno">  399</span>&#160;</div>
<div class="line"><a name="l00400"></a><span class="lineno">  400</span>&#160;            <span class="comment">// Compute the state/observation covariance</span></div>
<div class="line"><a name="l00401"></a><span class="lineno">  401</span>&#160;            <span class="comment">// Eq (3.219)</span></div>
<div class="line"><a name="l00402"></a><span class="lineno">  402</span>&#160;            gsl_matrix_set_zero(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0d210360e59328e219b5f37751baac35">Pxyi</a>);</div>
<div class="line"><a name="l00403"></a><span class="lineno">  403</span>&#160;            <span class="keywordflow">for</span>(j = 0 ; j &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a45062e1fd58806a27f0e4f75772e33d2">nbSamplesMeasure</a> ; ++j)</div>
<div class="line"><a name="l00404"></a><span class="lineno">  404</span>&#160;            {</div>
<div class="line"><a name="l00405"></a><span class="lineno">  405</span>&#160;                <span class="keywordflow">for</span>(i = 0 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> ; ++i)</div>
<div class="line"><a name="l00406"></a><span class="lineno">  406</span>&#160;                    s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a74e1d652fbafe8fe531bd6c61ef33ed8">temp_n_1</a>-&gt;data[i] = gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a90056d939d833550de52374098d3e348">sigmaPointsMeasure</a>,i,j) - s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0ead044a78987bebf3a2df35efdcead3">xi_mean</a>-&gt;data[i];</div>
<div class="line"><a name="l00407"></a><span class="lineno">  407</span>&#160;</div>
<div class="line"><a name="l00408"></a><span class="lineno">  408</span>&#160;                <span class="keywordflow">for</span>(i = 0 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a> ; ++i)</div>
<div class="line"><a name="l00409"></a><span class="lineno">  409</span>&#160;                    s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a00ab5d6e7673817ab87c143df0e3b698">temp_1_no</a>-&gt;data[i] = gsl_matrix_get(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#ac205dee75be43b32c76b0696d0182ab6">yi_prediction</a>,i,j) - s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#af0b49a1284aa6ce02d8fe249df41d434">yi_mean</a>-&gt;data[i];</div>
<div class="line"><a name="l00410"></a><span class="lineno">  410</span>&#160;</div>
<div class="line"><a name="l00411"></a><span class="lineno">  411</span>&#160;                gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a5cc587b3f1b1e905242a484df549e4a1">wc_aug_j</a>-&gt;data[j] , s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a74e1d652fbafe8fe531bd6c61ef33ed8">temp_n_1</a>, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a00ab5d6e7673817ab87c143df0e3b698">temp_1_no</a>, 1.0, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0d210360e59328e219b5f37751baac35">Pxyi</a>);</div>
<div class="line"><a name="l00412"></a><span class="lineno">  412</span>&#160;                <span class="comment">//gsl_matrix_add(s.Pxyi, s.temp_n_no);</span></div>
<div class="line"><a name="l00413"></a><span class="lineno">  413</span>&#160;            }</div>
<div class="line"><a name="l00414"></a><span class="lineno">  414</span>&#160;</div>
<div class="line"><a name="l00415"></a><span class="lineno">  415</span>&#160;            <span class="comment">/*gsl_matrix_set_zero(s.Pxyi);</span></div>
<div class="line"><a name="l00416"></a><span class="lineno">  416</span>&#160;<span class="comment">            for(j = 0 ; j &lt; p.nbSamples ; ++j)</span></div>
<div class="line"><a name="l00417"></a><span class="lineno">  417</span>&#160;<span class="comment">            {</span></div>
<div class="line"><a name="l00418"></a><span class="lineno">  418</span>&#160;<span class="comment">                for(i = 0 ; i &lt; p.n ; ++i)</span></div>
<div class="line"><a name="l00419"></a><span class="lineno">  419</span>&#160;<span class="comment">                    s.temp_n_1-&gt;data[i] = gsl_matrix_get(s.xi_prediction,i,j) - s.xi_mean-&gt;data[i];</span></div>
<div class="line"><a name="l00420"></a><span class="lineno">  420</span>&#160;<span class="comment"></span></div>
<div class="line"><a name="l00421"></a><span class="lineno">  421</span>&#160;<span class="comment">                for(i = 0 ; i &lt; p.no ; ++i)</span></div>
<div class="line"><a name="l00422"></a><span class="lineno">  422</span>&#160;<span class="comment">                    s.temp_1_no-&gt;data[i] = gsl_matrix_get(s.yi_prediction,i,j) - s.yi_mean-&gt;data[i];</span></div>
<div class="line"><a name="l00423"></a><span class="lineno">  423</span>&#160;<span class="comment"></span></div>
<div class="line"><a name="l00424"></a><span class="lineno">  424</span>&#160;<span class="comment">                gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, s.wc_j-&gt;data[j] , s.temp_n_1, s.temp_1_no, 1.0, s.Pxyi);</span></div>
<div class="line"><a name="l00425"></a><span class="lineno">  425</span>&#160;<span class="comment">                //gsl_matrix_add(s.Pxyi, s.temp_n_no);</span></div>
<div class="line"><a name="l00426"></a><span class="lineno">  426</span>&#160;<span class="comment">            }*/</span></div>
<div class="line"><a name="l00427"></a><span class="lineno">  427</span>&#160;</div>
<div class="line"><a name="l00428"></a><span class="lineno">  428</span>&#160;            <span class="comment">// Compute the Kalman gain,</span></div>
<div class="line"><a name="l00429"></a><span class="lineno">  429</span>&#160;            <span class="comment">// Eq (3.220)</span></div>
<div class="line"><a name="l00430"></a><span class="lineno">  430</span>&#160;</div>
<div class="line"><a name="l00431"></a><span class="lineno">  431</span>&#160;            <span class="comment">// We want to solve : Kxy . Sy Sy^T = Pxy</span></div>
<div class="line"><a name="l00432"></a><span class="lineno">  432</span>&#160;            <span class="comment">// Which can be solved using : Sy Sy^T Kxy^T = Pxy^T</span></div>
<div class="line"><a name="l00433"></a><span class="lineno">  433</span>&#160;            <span class="comment">// We have a number of no systems to solve of the form S x = Pxy^T[:,i] ; S^T Kxy[:,i] = x with S being triangular</span></div>
<div class="line"><a name="l00434"></a><span class="lineno">  434</span>&#160;            <span class="comment">// To solve this, we make use of gsl_blas_dtrsv</span></div>
<div class="line"><a name="l00435"></a><span class="lineno">  435</span>&#160;            <span class="keywordflow">for</span>(<span class="keywordtype">int</span> i = 0 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">n</a> ; ++i)</div>
<div class="line"><a name="l00436"></a><span class="lineno">  436</span>&#160;            {</div>
<div class="line"><a name="l00437"></a><span class="lineno">  437</span>&#160;              vec_view = gsl_matrix_row(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a0d210360e59328e219b5f37751baac35">Pxyi</a>, i);</div>
<div class="line"><a name="l00438"></a><span class="lineno">  438</span>&#160;                gsl_vector_memcpy(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a64e81fa0a22add1316bd66d03e70f105">temp_no</a>, &amp;vec_view.vector);</div>
<div class="line"><a name="l00439"></a><span class="lineno">  439</span>&#160;                gsl_blas_dtrsv(CblasLower,CblasNoTrans,CblasNonUnit,s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a70875ec1b32a4a73816c139ebe54d17e">Syi</a>,s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a64e81fa0a22add1316bd66d03e70f105">temp_no</a>);</div>
<div class="line"><a name="l00440"></a><span class="lineno">  440</span>&#160;                gsl_blas_dtrsv(CblasLower,CblasTrans,CblasNonUnit,s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a70875ec1b32a4a73816c139ebe54d17e">Syi</a>,s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a64e81fa0a22add1316bd66d03e70f105">temp_no</a>);</div>
<div class="line"><a name="l00441"></a><span class="lineno">  441</span>&#160;                <span class="comment">// And then copy the result in the ith row of Ki</span></div>
<div class="line"><a name="l00442"></a><span class="lineno">  442</span>&#160;                vec_view = gsl_matrix_row(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a43bbdfbb7c743522ebc18b2e299c7c1e">Ki</a>, i);</div>
<div class="line"><a name="l00443"></a><span class="lineno">  443</span>&#160;                gsl_vector_memcpy(&amp;vec_view.vector, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a64e81fa0a22add1316bd66d03e70f105">temp_no</a>);</div>
<div class="line"><a name="l00444"></a><span class="lineno">  444</span>&#160;            }</div>
<div class="line"><a name="l00445"></a><span class="lineno">  445</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) printf(<span class="stringliteral">&quot;Ki : \n&quot;</span>);</div>
<div class="line"><a name="l00446"></a><span class="lineno">  446</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) <a class="code" href="ukf__sr__state__ndim_8h.html#a2b6b76843e5a380b93a5cd4238a688a8">print_mat</a>(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a43bbdfbb7c743522ebc18b2e299c7c1e">Ki</a>);</div>
<div class="line"><a name="l00447"></a><span class="lineno">  447</span>&#160;</div>
<div class="line"><a name="l00448"></a><span class="lineno">  448</span>&#160;            <span class="comment">// Correction : correct the estimation of the state</span></div>
<div class="line"><a name="l00449"></a><span class="lineno">  449</span>&#160;            <span class="comment">// Eq. 3.221</span></div>
<div class="line"><a name="l00450"></a><span class="lineno">  450</span>&#160;            <span class="comment">// Compute the innovations</span></div>
<div class="line"><a name="l00451"></a><span class="lineno">  451</span>&#160;            <span class="keywordflow">for</span>(i = 0 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a> ; ++i)</div>
<div class="line"><a name="l00452"></a><span class="lineno">  452</span>&#160;                s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#add7a90f3736c8909a2e3abc3e4235ba7">ino_yi</a>-&gt;data[i] = yi-&gt;data[i] - s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#af0b49a1284aa6ce02d8fe249df41d434">yi_mean</a>-&gt;data[i];</div>
<div class="line"><a name="l00453"></a><span class="lineno">  453</span>&#160;            <span class="comment">// And correct the current state estimate</span></div>
<div class="line"><a name="l00454"></a><span class="lineno">  454</span>&#160;            gsl_blas_dgemv(CblasNoTrans, 1.0, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a43bbdfbb7c743522ebc18b2e299c7c1e">Ki</a>, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#add7a90f3736c8909a2e3abc3e4235ba7">ino_yi</a>, 1.0, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a9095ffca4f0bc5cbf7a42368cf7463d6">xi</a>);</div>
<div class="line"><a name="l00455"></a><span class="lineno">  455</span>&#160;</div>
<div class="line"><a name="l00456"></a><span class="lineno">  456</span>&#160;            <span class="comment">// Compute the matrix U = Kk Syk</span></div>
<div class="line"><a name="l00457"></a><span class="lineno">  457</span>&#160;            gsl_blas_dgemm(CblasNoTrans, CblasNoTrans, 1.0 , s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a43bbdfbb7c743522ebc18b2e299c7c1e">Ki</a>, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a70875ec1b32a4a73816c139ebe54d17e">Syi</a>, 0, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a903d42d54de210cc63826a6578d64b3c">U</a>);</div>
<div class="line"><a name="l00458"></a><span class="lineno">  458</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) printf(<span class="stringliteral">&quot;Matrix U : \n&quot;</span>);</div>
<div class="line"><a name="l00459"></a><span class="lineno">  459</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) <a class="code" href="ukf__sr__state__ndim_8h.html#a2b6b76843e5a380b93a5cd4238a688a8">print_mat</a>(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a903d42d54de210cc63826a6578d64b3c">U</a>);</div>
<div class="line"><a name="l00460"></a><span class="lineno">  460</span>&#160;</div>
<div class="line"><a name="l00461"></a><span class="lineno">  461</span>&#160;            <span class="comment">// Perform the cholesky updates of Sxi</span></div>
<div class="line"><a name="l00462"></a><span class="lineno">  462</span>&#160;            <span class="comment">// We perform p.no updates rank-1 update with the columns of U</span></div>
<div class="line"><a name="l00463"></a><span class="lineno">  463</span>&#160;            <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) printf(<span class="stringliteral">&quot;Before cholupdate 3 \n&quot;</span>);</div>
<div class="line"><a name="l00464"></a><span class="lineno">  464</span>&#160;            <span class="keywordflow">for</span>(i = 0 ; i &lt; p.<a class="code" href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">no</a> ; ++i)</div>
<div class="line"><a name="l00465"></a><span class="lineno">  465</span>&#160;            {</div>
<div class="line"><a name="l00466"></a><span class="lineno">  466</span>&#160;              vec_view = gsl_matrix_column(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a903d42d54de210cc63826a6578d64b3c">U</a>, i);</div>
<div class="line"><a name="l00467"></a><span class="lineno">  467</span>&#160;                gsl_vector_memcpy(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a2158f781908727006d11975d94c55ad0">temp_n</a>, &amp;vec_view.vector);</div>
<div class="line"><a name="l00468"></a><span class="lineno">  468</span>&#160;                <a class="code" href="namespaceukf_1_1math.html#ac921eed0b4618cb2e860cf712c676675">ukf::math::choleskyUpdate</a>(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">Sxi</a>, -1.0, s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#a2158f781908727006d11975d94c55ad0">temp_n</a>);</div>
<div class="line"><a name="l00469"></a><span class="lineno">  469</span>&#160;                <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) printf(<span class="stringliteral">&quot;After cholupdate %i\n&quot;</span>, i);</div>
<div class="line"><a name="l00470"></a><span class="lineno">  470</span>&#160;                <span class="keywordflow">if</span>(<a class="code" href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a>) <a class="code" href="ukf__sr__state__ndim_8h.html#a2b6b76843e5a380b93a5cd4238a688a8">print_mat</a>(s.<a class="code" href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">Sxi</a>);</div>
<div class="line"><a name="l00471"></a><span class="lineno">  471</span>&#160;            }</div>
<div class="line"><a name="l00472"></a><span class="lineno">  472</span>&#160;</div>
<div class="line"><a name="l00473"></a><span class="lineno">  473</span>&#160;            <span class="comment">//printf(&quot;After cholupdate %i\n&quot;, i);</span></div>
<div class="line"><a name="l00474"></a><span class="lineno">  474</span>&#160;            <span class="comment">//print_mat(s.Sxi);</span></div>
<div class="line"><a name="l00475"></a><span class="lineno">  475</span>&#160;            <span class="comment">//if(DEBUGMODE) printf(&quot;\n&quot;);</span></div>
<div class="line"><a name="l00476"></a><span class="lineno">  476</span>&#160;</div>
<div class="line"><a name="l00477"></a><span class="lineno">  477</span>&#160;            <span class="comment">//</span></div>
<div class="line"><a name="l00478"></a><span class="lineno">  478</span>&#160;            <span class="comment">//            // Update of the process noise</span></div>
<div class="line"><a name="l00479"></a><span class="lineno">  479</span>&#160;            <span class="comment">//            switch(p.process_noise_type)</span></div>
<div class="line"><a name="l00480"></a><span class="lineno">  480</span>&#160;            <span class="comment">//            {</span></div>
<div class="line"><a name="l00481"></a><span class="lineno">  481</span>&#160;            <span class="comment">//            case ukf::UKF_PROCESS_FIXED:</span></div>
<div class="line"><a name="l00482"></a><span class="lineno">  482</span>&#160;            <span class="comment">//                //nothing to do</span></div>
<div class="line"><a name="l00483"></a><span class="lineno">  483</span>&#160;            <span class="comment">//                break;</span></div>
<div class="line"><a name="l00484"></a><span class="lineno">  484</span>&#160;            <span class="comment">//            case ukf::UKF_PROCESS_RLS:</span></div>
<div class="line"><a name="l00485"></a><span class="lineno">  485</span>&#160;            <span class="comment">//                gsl_matrix_memcpy(s.Pvvi, s.Pxxi);</span></div>
<div class="line"><a name="l00486"></a><span class="lineno">  486</span>&#160;            <span class="comment">//                gsl_matrix_scale(s.Pvvi, 1.0/p.process_noise-1.0);</span></div>
<div class="line"><a name="l00487"></a><span class="lineno">  487</span>&#160;            <span class="comment">//                gsl_matrix_memcpy(s.cholPvvi, s.Pvvi);</span></div>
<div class="line"><a name="l00488"></a><span class="lineno">  488</span>&#160;            <span class="comment">//                gsl_linalg_cholesky_decomp(s.cholPvvi);</span></div>
<div class="line"><a name="l00489"></a><span class="lineno">  489</span>&#160;            <span class="comment">//                for(j = 0 ; j &lt; p.n ; j++)</span></div>
<div class="line"><a name="l00490"></a><span class="lineno">  490</span>&#160;            <span class="comment">//                    for(k = j+1 ; k &lt; p.n ; k++)</span></div>
<div class="line"><a name="l00491"></a><span class="lineno">  491</span>&#160;            <span class="comment">//                        gsl_matrix_set(s.cholPvvi,j,k,0.0);</span></div>
<div class="line"><a name="l00492"></a><span class="lineno">  492</span>&#160;            <span class="comment">//                break;</span></div>
<div class="line"><a name="l00493"></a><span class="lineno">  493</span>&#160;            <span class="comment">//            default:</span></div>
<div class="line"><a name="l00494"></a><span class="lineno">  494</span>&#160;            <span class="comment">//                printf(&quot;Warning : Unrecognized process noise type\n&quot;);</span></div>
<div class="line"><a name="l00495"></a><span class="lineno">  495</span>&#160;            <span class="comment">//            }</span></div>
<div class="line"><a name="l00496"></a><span class="lineno">  496</span>&#160;</div>
<div class="line"><a name="l00497"></a><span class="lineno">  497</span>&#160;</div>
<div class="line"><a name="l00498"></a><span class="lineno">  498</span>&#160;        }</div>
<div class="line"><a name="l00499"></a><span class="lineno">  499</span>&#160;</div>
<div class="line"><a name="l00504"></a><span class="lineno">  504</span>&#160;        <span class="keyword">template</span> &lt;<span class="keyword">typename</span> FunctProcess, <span class="keyword">typename</span> FunctObservation&gt;</div>
<div class="line"><a name="l00505"></a><span class="lineno"><a class="line" href="namespaceukf_1_1srstate.html#a22c2755916c2483faa7049811ce6bc8a">  505</a></span>&#160;                <span class="keyword">inline</span> <span class="keywordtype">void</span> <a class="code" href="namespaceukf_1_1srstate.html#a22c2755916c2483faa7049811ce6bc8a">ukf_evaluate</a>(<a class="code" href="structukf_1_1srstate_1_1ukf__param.html">ukf_param</a> &amp;p, <a class="code" href="structukf_1_1srstate_1_1ukf__state.html">ukf_state</a> &amp;s, FunctProcess f, FunctObservation h, gsl_vector* yi)</div>
<div class="line"><a name="l00506"></a><span class="lineno">  506</span>&#160;        {</div>
<div class="line"><a name="l00507"></a><span class="lineno">  507</span>&#160;</div>
<div class="line"><a name="l00508"></a><span class="lineno">  508</span>&#160;            <span class="comment">//          int i,j,k;</span></div>
<div class="line"><a name="l00509"></a><span class="lineno">  509</span>&#160;            <span class="comment">//            // ************************************************** //</span></div>
<div class="line"><a name="l00510"></a><span class="lineno">  510</span>&#160;            <span class="comment">//            // ************ Compute the sigma points ************ //</span></div>
<div class="line"><a name="l00511"></a><span class="lineno">  511</span>&#160;            <span class="comment">//            // ************************************************** //</span></div>
<div class="line"><a name="l00512"></a><span class="lineno">  512</span>&#160;            <span class="comment">//</span></div>
<div class="line"><a name="l00513"></a><span class="lineno">  513</span>&#160;            <span class="comment">//            // 0 - Compute the Cholesky decomposition of s.Pxxi</span></div>
<div class="line"><a name="l00514"></a><span class="lineno">  514</span>&#160;            <span class="comment">//            gsl_matrix_memcpy(s.cholPxxi, s.Pxxi);</span></div>
<div class="line"><a name="l00515"></a><span class="lineno">  515</span>&#160;            <span class="comment">//            gsl_linalg_cholesky_decomp(s.cholPxxi);</span></div>
<div class="line"><a name="l00516"></a><span class="lineno">  516</span>&#160;            <span class="comment">//            // Set all the elements of cholPvvi strictly above the diagonal to zero</span></div>
<div class="line"><a name="l00517"></a><span class="lineno">  517</span>&#160;            <span class="comment">//            for(j = 0 ; j &lt; p.n ; j++)</span></div>
<div class="line"><a name="l00518"></a><span class="lineno">  518</span>&#160;            <span class="comment">//                for(k = j+1 ; k &lt; p.n ; k++)</span></div>
<div class="line"><a name="l00519"></a><span class="lineno">  519</span>&#160;            <span class="comment">//                    gsl_matrix_set(s.cholPxxi,j,k,0.0);</span></div>
<div class="line"><a name="l00520"></a><span class="lineno">  520</span>&#160;            <span class="comment">//</span></div>
<div class="line"><a name="l00521"></a><span class="lineno">  521</span>&#160;            <span class="comment">//            // 1- Compute the sigma points,</span></div>
<div class="line"><a name="l00522"></a><span class="lineno">  522</span>&#160;            <span class="comment">//            // Equation (3.170)</span></div>
<div class="line"><a name="l00523"></a><span class="lineno">  523</span>&#160;            <span class="comment">//            // sigmapoint_j = x_(i-1)</span></div>
<div class="line"><a name="l00524"></a><span class="lineno">  524</span>&#160;            <span class="comment">//            // sigmapoint_j = x_(i-1) + gamma * sqrt(P_i-1)_j for 1 &lt;= j &lt;= n</span></div>
<div class="line"><a name="l00525"></a><span class="lineno">  525</span>&#160;            <span class="comment">//            // sigmapoint_j = x_(i-1) - gamma * sqrt(P_i-1)_(j-(n+1)) for n+1 &lt;= j &lt;= 2n</span></div>
<div class="line"><a name="l00526"></a><span class="lineno">  526</span>&#160;            <span class="comment">//            gsl_matrix_set_col(s.sigmaPoints, 0, s.xi);</span></div>
<div class="line"><a name="l00527"></a><span class="lineno">  527</span>&#160;            <span class="comment">//            for(j = 1 ; j &lt; p.n+1 ; ++j)</span></div>
<div class="line"><a name="l00528"></a><span class="lineno">  528</span>&#160;            <span class="comment">//                for(i = 0 ; i &lt; p.n ; ++i)</span></div>
<div class="line"><a name="l00529"></a><span class="lineno">  529</span>&#160;            <span class="comment">//                {</span></div>
<div class="line"><a name="l00530"></a><span class="lineno">  530</span>&#160;            <span class="comment">//                    gsl_matrix_set(s.sigmaPoints,i,j, s.xi-&gt;data[i] + p.gamma * gsl_matrix_get(s.cholPxxi, i, j-1));</span></div>
<div class="line"><a name="l00531"></a><span class="lineno">  531</span>&#160;            <span class="comment">//                    gsl_matrix_set(s.sigmaPoints,i,j+p.n, s.xi-&gt;data[i] - p.gamma * gsl_matrix_get(s.cholPxxi, i, j-1));</span></div>
<div class="line"><a name="l00532"></a><span class="lineno">  532</span>&#160;            <span class="comment">//                }</span></div>
<div class="line"><a name="l00533"></a><span class="lineno">  533</span>&#160;            <span class="comment">//</span></div>
<div class="line"><a name="l00534"></a><span class="lineno">  534</span>&#160;            <span class="comment">//            /**********************************/</span></div>
<div class="line"><a name="l00535"></a><span class="lineno">  535</span>&#160;            <span class="comment">//            /***** Time update equations  *****/</span></div>
<div class="line"><a name="l00536"></a><span class="lineno">  536</span>&#160;            <span class="comment">//            /**********************************/</span></div>
<div class="line"><a name="l00537"></a><span class="lineno">  537</span>&#160;            <span class="comment">//</span></div>
<div class="line"><a name="l00538"></a><span class="lineno">  538</span>&#160;            <span class="comment">//            // Time update equations</span></div>
<div class="line"><a name="l00539"></a><span class="lineno">  539</span>&#160;            <span class="comment">//            // 0 - Compute the image of the sigma points and the mean of these images</span></div>
<div class="line"><a name="l00540"></a><span class="lineno">  540</span>&#160;            <span class="comment">//            gsl_vector_set_zero(s.xi_mean);</span></div>
<div class="line"><a name="l00541"></a><span class="lineno">  541</span>&#160;            <span class="comment">//            for(j = 0 ; j &lt; p.nbSamples ; ++j)</span></div>
<div class="line"><a name="l00542"></a><span class="lineno">  542</span>&#160;            <span class="comment">//            {</span></div>
<div class="line"><a name="l00543"></a><span class="lineno">  543</span>&#160;            <span class="comment">//                gsl_matrix_get_col(s.sigmaPoint, s.sigmaPoints, j);</span></div>
<div class="line"><a name="l00544"></a><span class="lineno">  544</span>&#160;            <span class="comment">//                f(s.params, s.sigmaPoint, &amp;gsl_matrix_column(s.xi_prediction,j).vector);</span></div>
<div class="line"><a name="l00545"></a><span class="lineno">  545</span>&#160;            <span class="comment">//</span></div>
<div class="line"><a name="l00546"></a><span class="lineno">  546</span>&#160;            <span class="comment">//                // Update the mean, Eq (3.172)</span></div>
<div class="line"><a name="l00547"></a><span class="lineno">  547</span>&#160;            <span class="comment">//                for(i = 0 ; i &lt; p.n ; ++i)</span></div>
<div class="line"><a name="l00548"></a><span class="lineno">  548</span>&#160;            <span class="comment">//                    s.xi_mean-&gt;data[i] += s.wm_j-&gt;data[j] * gsl_matrix_get(s.xi_prediction,i,j);</span></div>
<div class="line"><a name="l00549"></a><span class="lineno">  549</span>&#160;            <span class="comment">//            }</span></div>
<div class="line"><a name="l00550"></a><span class="lineno">  550</span>&#160;            <span class="comment">//</span></div>
<div class="line"><a name="l00551"></a><span class="lineno">  551</span>&#160;            <span class="comment">//            // 1 - Compute the covariance of the images and add the process noise,</span></div>
<div class="line"><a name="l00552"></a><span class="lineno">  552</span>&#160;            <span class="comment">//            // Equation (3.173)</span></div>
<div class="line"><a name="l00553"></a><span class="lineno">  553</span>&#160;            <span class="comment">//            // Warning, s.Pxxi will now hold P_xk^-</span></div>
<div class="line"><a name="l00554"></a><span class="lineno">  554</span>&#160;            <span class="comment">//            gsl_matrix_set_zero(s.Pxxi);</span></div>
<div class="line"><a name="l00555"></a><span class="lineno">  555</span>&#160;            <span class="comment">//            for(j = 0 ; j &lt; p.nbSamples ; ++j)</span></div>
<div class="line"><a name="l00556"></a><span class="lineno">  556</span>&#160;            <span class="comment">//            {</span></div>
<div class="line"><a name="l00557"></a><span class="lineno">  557</span>&#160;            <span class="comment">//                for(i = 0 ; i &lt; p.n ; ++i)</span></div>
<div class="line"><a name="l00558"></a><span class="lineno">  558</span>&#160;            <span class="comment">//                    s.temp_n_1-&gt;data[i] = gsl_matrix_get(s.xi_prediction,i,j) - s.xi_mean-&gt;data[i];</span></div>
<div class="line"><a name="l00559"></a><span class="lineno">  559</span>&#160;            <span class="comment">//</span></div>
<div class="line"><a name="l00560"></a><span class="lineno">  560</span>&#160;            <span class="comment">//                gsl_blas_dgemm(CblasNoTrans, CblasTrans, s.wc_j-&gt;data[j] , s.temp_n_1, s.temp_n_1, 0, s.temp_n_n);</span></div>
<div class="line"><a name="l00561"></a><span class="lineno">  561</span>&#160;            <span class="comment">//                gsl_matrix_add(s.Pxxi, s.temp_n_n);</span></div>
<div class="line"><a name="l00562"></a><span class="lineno">  562</span>&#160;            <span class="comment">//            }</span></div>
<div class="line"><a name="l00563"></a><span class="lineno">  563</span>&#160;            <span class="comment">//            // Add the covariance of the evolution noise</span></div>
<div class="line"><a name="l00564"></a><span class="lineno">  564</span>&#160;            <span class="comment">//            gsl_matrix_add(s.Pxxi, s.Pvvi);</span></div>
<div class="line"><a name="l00565"></a><span class="lineno">  565</span>&#160;            <span class="comment">//</span></div>
<div class="line"><a name="l00566"></a><span class="lineno">  566</span>&#160;            <span class="comment">//            // Augment sigma points</span></div>
<div class="line"><a name="l00567"></a><span class="lineno">  567</span>&#160;            <span class="comment">//            // Equation 3.174</span></div>
<div class="line"><a name="l00568"></a><span class="lineno">  568</span>&#160;            <span class="comment">//            // First put the images of the initial sigma points</span></div>
<div class="line"><a name="l00569"></a><span class="lineno">  569</span>&#160;            <span class="comment">//            gsl_matrix_memcpy(&amp;gsl_matrix_submatrix(s.sigmaPointsMeasure, 0, 0, p.n, p.nbSamples).matrix, s.xi_prediction);</span></div>
<div class="line"><a name="l00570"></a><span class="lineno">  570</span>&#160;            <span class="comment">//            // And add the additional sigma points eq. (7.56)</span></div>
<div class="line"><a name="l00571"></a><span class="lineno">  571</span>&#160;            <span class="comment">//            for(j = 0 ; j &lt; p.n ; ++j)</span></div>
<div class="line"><a name="l00572"></a><span class="lineno">  572</span>&#160;            <span class="comment">//            {</span></div>
<div class="line"><a name="l00573"></a><span class="lineno">  573</span>&#160;            <span class="comment">//                for(i = 0 ; i &lt; p.n ; ++i)</span></div>
<div class="line"><a name="l00574"></a><span class="lineno">  574</span>&#160;            <span class="comment">//                {</span></div>
<div class="line"><a name="l00575"></a><span class="lineno">  575</span>&#160;            <span class="comment">//                    gsl_matrix_set(s.sigmaPointsMeasure, i, j+p.nbSamples, gsl_matrix_get(s.xi_prediction,i,0)+p.gamma_aug*gsl_matrix_get(s.cholPvvi,i,j));</span></div>
<div class="line"><a name="l00576"></a><span class="lineno">  576</span>&#160;            <span class="comment">//                    gsl_matrix_set(s.sigmaPointsMeasure, i, j+p.nbSamples+p.n, gsl_matrix_get(s.xi_prediction,i,0)-p.gamma_aug*gsl_matrix_get(s.cholPvvi,i,j));</span></div>
<div class="line"><a name="l00577"></a><span class="lineno">  577</span>&#160;            <span class="comment">//                }</span></div>
<div class="line"><a name="l00578"></a><span class="lineno">  578</span>&#160;            <span class="comment">//            }</span></div>
<div class="line"><a name="l00579"></a><span class="lineno">  579</span>&#160;            <span class="comment">//</span></div>
<div class="line"><a name="l00580"></a><span class="lineno">  580</span>&#160;            <span class="comment">//            // Compute the image of the sigma points through the observation equation</span></div>
<div class="line"><a name="l00581"></a><span class="lineno">  581</span>&#160;            <span class="comment">//            // eq (3.175)</span></div>
<div class="line"><a name="l00582"></a><span class="lineno">  582</span>&#160;            <span class="comment">//            gsl_vector_set_zero(yi);</span></div>
<div class="line"><a name="l00583"></a><span class="lineno">  583</span>&#160;            <span class="comment">//            for(j = 0 ; j &lt; p.nbSamplesMeasure ; ++j)</span></div>
<div class="line"><a name="l00584"></a><span class="lineno">  584</span>&#160;            <span class="comment">//            {</span></div>
<div class="line"><a name="l00585"></a><span class="lineno">  585</span>&#160;            <span class="comment">//                gsl_matrix_get_col(s.sigmaPointMeasure, s.sigmaPointsMeasure, j);</span></div>
<div class="line"><a name="l00586"></a><span class="lineno">  586</span>&#160;            <span class="comment">//                h(s.sigmaPointMeasure, &amp;gsl_matrix_column(s.yi_prediction,j).vector);</span></div>
<div class="line"><a name="l00587"></a><span class="lineno">  587</span>&#160;            <span class="comment">//</span></div>
<div class="line"><a name="l00588"></a><span class="lineno">  588</span>&#160;            <span class="comment">//                // Update the mean , eq (3.176)</span></div>
<div class="line"><a name="l00589"></a><span class="lineno">  589</span>&#160;            <span class="comment">//                for(i = 0 ; i &lt; p.no ; ++i)</span></div>
<div class="line"><a name="l00590"></a><span class="lineno">  590</span>&#160;            <span class="comment">//                    yi-&gt;data[i] += s.wm_aug_j-&gt;data[j] * gsl_matrix_get(s.yi_prediction,i,j);</span></div>
<div class="line"><a name="l00591"></a><span class="lineno">  591</span>&#160;            <span class="comment">//            }</span></div>
<div class="line"><a name="l00592"></a><span class="lineno">  592</span>&#160;        }</div>
<div class="line"><a name="l00593"></a><span class="lineno">  593</span>&#160;    } <span class="comment">// srstate</span></div>
<div class="line"><a name="l00594"></a><span class="lineno">  594</span>&#160;} <span class="comment">// ukf</span></div>
<div class="line"><a name="l00595"></a><span class="lineno">  595</span>&#160;</div>
<div class="line"><a name="l00596"></a><span class="lineno">  596</span>&#160;<span class="preprocessor">#endif // UKF_SR_STATE_NDIM_H</span></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_aa702fccfc9136bd1d178e03cabf5cee8"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#aa702fccfc9136bd1d178e03cabf5cee8">ukf::srstate::ukf_state::Sxi</a></div><div class="ttdeci">gsl_matrix * Sxi</div><div class="ttdef"><b>Definition:</b> ukf_types.h:832</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a0876b435bf2ca99901c779bde40d9004"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a0876b435bf2ca99901c779bde40d9004">ukf::srstate::ukf_state::xi_prediction</a></div><div class="ttdeci">gsl_matrix * xi_prediction</div><div class="ttdef"><b>Definition:</b> ukf_types.h:830</div></div>
<div class="ttc" id="namespaceukf_1_1srstate_html_a3a2cccd808cea8666029bcbfc44ba460"><div class="ttname"><a href="namespaceukf_1_1srstate.html#a3a2cccd808cea8666029bcbfc44ba460">ukf::srstate::ukf_free</a></div><div class="ttdeci">void ukf_free(ukf_param &amp;p, ukf_state &amp;s)</div><div class="ttdoc">Free of memory allocation. </div><div class="ttdef"><b>Definition:</b> ukf_sr_state_ndim.h:180</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_af0b49a1284aa6ce02d8fe249df41d434"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#af0b49a1284aa6ce02d8fe249df41d434">ukf::srstate::ukf_state::yi_mean</a></div><div class="ttdeci">gsl_vector * yi_mean</div><div class="ttdef"><b>Definition:</b> ukf_types.h:836</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a2158f781908727006d11975d94c55ad0"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a2158f781908727006d11975d94c55ad0">ukf::srstate::ukf_state::temp_n</a></div><div class="ttdeci">gsl_vector * temp_n</div><div class="ttdef"><b>Definition:</b> ukf_types.h:860</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a903d42d54de210cc63826a6578d64b3c"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a903d42d54de210cc63826a6578d64b3c">ukf::srstate::ukf_state::U</a></div><div class="ttdeci">gsl_matrix * U</div><div class="ttdef"><b>Definition:</b> ukf_types.h:849</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_afee48e0e83f20d7d46a478a9b436a42b"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#afee48e0e83f20d7d46a478a9b436a42b">ukf::srstate::ukf_state::Sni</a></div><div class="ttdeci">gsl_matrix * Sni</div><div class="ttdef"><b>Definition:</b> ukf_types.h:839</div></div>
<div class="ttc" id="ukf__sr__state__ndim_8h_html_a2b6b76843e5a380b93a5cd4238a688a8"><div class="ttname"><a href="ukf__sr__state__ndim_8h.html#a2b6b76843e5a380b93a5cd4238a688a8">print_mat</a></div><div class="ttdeci">void print_mat(const gsl_matrix *A)</div><div class="ttdef"><b>Definition:</b> ukf_sr_state_ndim.h:32</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_ae5bd148e7b9dd43dca85f62058d5013f"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#ae5bd148e7b9dd43dca85f62058d5013f">ukf::srstate::ukf_state::wm_aug_j</a></div><div class="ttdeci">gsl_vector * wm_aug_j</div><div class="ttdef"><b>Definition:</b> ukf_types.h:854</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html_a93c3981c8518153ce89bc868fb071267"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html#a93c3981c8518153ce89bc868fb071267">ukf::srstate::ukf_param::measurement_noise</a></div><div class="ttdeci">double measurement_noise</div><div class="ttdoc">Covariance of the observation noise. </div><div class="ttdef"><b>Definition:</b> ukf_types.h:816</div></div>
<div class="ttc" id="ukf__sr__state__ndim_8h_html_a7cfcd827e44f3bbf3fff23fbac44dfc8"><div class="ttname"><a href="ukf__sr__state__ndim_8h.html#a7cfcd827e44f3bbf3fff23fbac44dfc8">print_vec</a></div><div class="ttdeci">void print_vec(const gsl_vector *A)</div><div class="ttdef"><b>Definition:</b> ukf_sr_state_ndim.h:43</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a0d210360e59328e219b5f37751baac35"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a0d210360e59328e219b5f37751baac35">ukf::srstate::ukf_state::Pxyi</a></div><div class="ttdeci">gsl_matrix * Pxyi</div><div class="ttdef"><b>Definition:</b> ukf_types.h:841</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html">ukf::srstate::ukf_param</a></div><div class="ttdoc">Structure holding the parameters of the statistical linearization. </div><div class="ttdef"><b>Definition:</b> ukf_types.h:749</div></div>
<div class="ttc" id="namespaceukf_1_1math_html_ac921eed0b4618cb2e860cf712c676675"><div class="ttname"><a href="namespaceukf_1_1math.html#ac921eed0b4618cb2e860cf712c676675">ukf::math::choleskyUpdate</a></div><div class="ttdeci">void choleskyUpdate(gsl_matrix *sigmaTheta, double alpha, gsl_vector *x)</div><div class="ttdoc">This function performs a cholesky update according to Strange et al.(2007) </div><div class="ttdef"><b>Definition:</b> ukf_math.h:64</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a5cc587b3f1b1e905242a484df549e4a1"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a5cc587b3f1b1e905242a484df549e4a1">ukf::srstate::ukf_state::wc_aug_j</a></div><div class="ttdeci">gsl_vector * wc_aug_j</div><div class="ttdef"><b>Definition:</b> ukf_types.h:855</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a051b7e63dd546f62f0c57df7fac63efb"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a051b7e63dd546f62f0c57df7fac63efb">ukf::srstate::ukf_state::temp_no_1</a></div><div class="ttdeci">gsl_matrix * temp_no_1</div><div class="ttdef"><b>Definition:</b> ukf_types.h:870</div></div>
<div class="ttc" id="namespaceukf_1_1srstate_html_aff3bcb491d70b295c47a7644038e7ed2"><div class="ttname"><a href="namespaceukf_1_1srstate.html#aff3bcb491d70b295c47a7644038e7ed2">ukf::srstate::ukf_init</a></div><div class="ttdeci">void ukf_init(ukf_param &amp;p, ukf_state &amp;s)</div><div class="ttdoc">Allocation of the vectors/matrices and initialization. </div><div class="ttdef"><b>Definition:</b> ukf_sr_state_ndim.h:65</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a458721ed2b8869be17e7589024c6bac3"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a458721ed2b8869be17e7589024c6bac3">ukf::srstate::ukf_state::sigmaPoints</a></div><div class="ttdeci">gsl_matrix * sigmaPoints</div><div class="ttdef"><b>Definition:</b> ukf_types.h:844</div></div>
<div class="ttc" id="ukf__sr__state__ndim_8h_html_a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d"><div class="ttname"><a href="ukf__sr__state__ndim_8h.html#a9c4d4f46e70cc3b7dee9a2fe9c1bbf4d">DEBUGMODE</a></div><div class="ttdeci">#define DEBUGMODE</div><div class="ttdef"><b>Definition:</b> ukf_sr_state_ndim.h:20</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a64e81fa0a22add1316bd66d03e70f105"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a64e81fa0a22add1316bd66d03e70f105">ukf::srstate::ukf_state::temp_no</a></div><div class="ttdeci">gsl_vector * temp_no</div><div class="ttdef"><b>Definition:</b> ukf_types.h:861</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html_ab77dd9bb480fee611664a4fab007030a"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html#ab77dd9bb480fee611664a4fab007030a">ukf::srstate::ukf_param::beta</a></div><div class="ttdeci">double beta</div><div class="ttdoc">Non negative weights used to introduce knowledge about the higher order moments of the distribution...</div><div class="ttdef"><b>Definition:</b> ukf_types.h:764</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a00ab5d6e7673817ab87c143df0e3b698"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a00ab5d6e7673817ab87c143df0e3b698">ukf::srstate::ukf_state::temp_1_no</a></div><div class="ttdeci">gsl_matrix * temp_1_no</div><div class="ttdef"><b>Definition:</b> ukf_types.h:871</div></div>
<div class="ttc" id="namespaceukf_1_1srstate_html_a22c2755916c2483faa7049811ce6bc8a"><div class="ttname"><a href="namespaceukf_1_1srstate.html#a22c2755916c2483faa7049811ce6bc8a">ukf::srstate::ukf_evaluate</a></div><div class="ttdeci">void ukf_evaluate(ukf_param &amp;p, ukf_state &amp;s, FunctProcess f, FunctObservation h, gsl_vector *yi)</div><div class="ttdoc">Evaluation of the output from the sigma points. </div><div class="ttdef"><b>Definition:</b> ukf_sr_state_ndim.h:505</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a74e1d652fbafe8fe531bd6c61ef33ed8"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a74e1d652fbafe8fe531bd6c61ef33ed8">ukf::srstate::ukf_state::temp_n_1</a></div><div class="ttdeci">gsl_matrix * temp_n_1</div><div class="ttdef"><b>Definition:</b> ukf_types.h:864</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html_a4802c63baa1362da178bd2456a47704b"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html#a4802c63baa1362da178bd2456a47704b">ukf::srstate::ukf_param::lambda_aug</a></div><div class="ttdeci">double lambda_aug</div><div class="ttdef"><b>Definition:</b> ukf_types.h:770</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a483422efe8d15e91ff527261fbfbbbb7"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a483422efe8d15e91ff527261fbfbbbb7">ukf::srstate::ukf_state::temp_n_no</a></div><div class="ttdeci">gsl_matrix * temp_n_no</div><div class="ttdef"><b>Definition:</b> ukf_types.h:867</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a231b41faf198e81926477756e3f2c2e5"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a231b41faf198e81926477756e3f2c2e5">ukf::srstate::ukf_state::temp_1_n</a></div><div class="ttdeci">gsl_matrix * temp_1_n</div><div class="ttdef"><b>Definition:</b> ukf_types.h:865</div></div>
<div class="ttc" id="ukf__types_8h_html"><div class="ttname"><a href="ukf__types_8h.html">ukf_types.h</a></div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_ae224406380a3ab3aff38a97a874c08f3"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#ae224406380a3ab3aff38a97a874c08f3">ukf::srstate::ukf_state::wc_j</a></div><div class="ttdeci">gsl_vector * wc_j</div><div class="ttdef"><b>Definition:</b> ukf_types.h:852</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a698d2cd6e4da73ca0b4831e09f9c4419"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a698d2cd6e4da73ca0b4831e09f9c4419">ukf::srstate::ukf_state::sigmaPoint</a></div><div class="ttdeci">gsl_vector * sigmaPoint</div><div class="ttdef"><b>Definition:</b> ukf_types.h:843</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html_a91ac481e996d091fdb23db9eab23013e"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html#a91ac481e996d091fdb23db9eab23013e">ukf::srstate::ukf_param::process_noise</a></div><div class="ttdeci">double process_noise</div><div class="ttdoc">Parameter used for the evolution noise. </div><div class="ttdef"><b>Definition:</b> ukf_types.h:811</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html">ukf::srstate::ukf_state</a></div><div class="ttdoc">Structure holding the matrices manipulated by the statistical linearization in the vectorial case for...</div><div class="ttdef"><b>Definition:</b> ukf_types.h:825</div></div>
<div class="ttc" id="ukf__math_8h_html"><div class="ttname"><a href="ukf__math_8h.html">ukf_math.h</a></div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html_a41231af7e574722fee0a575ce989e434"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html#a41231af7e574722fee0a575ce989e434">ukf::srstate::ukf_param::kpa</a></div><div class="ttdeci">double kpa</div><div class="ttdoc">,  is a good choice. According to van der Merwe, its value is not critical </div><div class="ttdef"><b>Definition:</b> ukf_types.h:754</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a43bbdfbb7c743522ebc18b2e299c7c1e"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a43bbdfbb7c743522ebc18b2e299c7c1e">ukf::srstate::ukf_state::Ki</a></div><div class="ttdeci">gsl_matrix * Ki</div><div class="ttdef"><b>Definition:</b> ukf_types.h:857</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a961545fff86f5f611e5a64f76166be0c"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a961545fff86f5f611e5a64f76166be0c">ukf::srstate::ukf_state::temp_no_no</a></div><div class="ttdeci">gsl_matrix * temp_no_no</div><div class="ttdef"><b>Definition:</b> ukf_types.h:869</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html_a45062e1fd58806a27f0e4f75772e33d2"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html#a45062e1fd58806a27f0e4f75772e33d2">ukf::srstate::ukf_param::nbSamplesMeasure</a></div><div class="ttdeci">int nbSamplesMeasure</div><div class="ttdoc"> Number of sigma-points </div><div class="ttdef"><b>Definition:</b> ukf_types.h:791</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html_a5dc27e0d7ae329c6456efdeb0c359a39"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html#a5dc27e0d7ae329c6456efdeb0c359a39">ukf::srstate::ukf_param::gamma</a></div><div class="ttdeci">double gamma</div><div class="ttdoc"> </div><div class="ttdef"><b>Definition:</b> ukf_types.h:775</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a0ead044a78987bebf3a2df35efdcead3"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a0ead044a78987bebf3a2df35efdcead3">ukf::srstate::ukf_state::xi_mean</a></div><div class="ttdeci">gsl_vector * xi_mean</div><div class="ttdef"><b>Definition:</b> ukf_types.h:831</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a923a0facfe402f098a4da50e8f184f61"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a923a0facfe402f098a4da50e8f184f61">ukf::srstate::ukf_state::params</a></div><div class="ttdeci">gsl_vector * params</div><div class="ttdef"><b>Definition:</b> ukf_types.h:827</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html_a55760e3aa010ff582f27067c6168972a"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html#a55760e3aa010ff582f27067c6168972a">ukf::srstate::ukf_param::alpha</a></div><div class="ttdeci">double alpha</div><div class="ttdoc"> : &quot;Size&quot; of sigma-point distribution. Should be small if the function is strongly non-linear ...</div><div class="ttdef"><b>Definition:</b> ukf_types.h:759</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html_a2a382b59ee2aefa913f3bc24dc70bb44"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html#a2a382b59ee2aefa913f3bc24dc70bb44">ukf::srstate::ukf_param::nbSamples</a></div><div class="ttdeci">int nbSamples</div><div class="ttdoc"> Number of sigma-points </div><div class="ttdef"><b>Definition:</b> ukf_types.h:786</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_abaf7742e033e303907f9084a89f22301"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#abaf7742e033e303907f9084a89f22301">ukf::srstate::ukf_state::temp_2nno_no</a></div><div class="ttdeci">gsl_matrix * temp_2nno_no</div><div class="ttdef"><b>Definition:</b> ukf_types.h:874</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a0cd68e2eff4ab4feed22bcdb8ffcb9b8"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a0cd68e2eff4ab4feed22bcdb8ffcb9b8">ukf::srstate::ukf_state::temp_n_n</a></div><div class="ttdeci">gsl_matrix * temp_n_n</div><div class="ttdef"><b>Definition:</b> ukf_types.h:863</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html_ae2e4444fe304b2908ceaa1f264e45ba2"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html#ae2e4444fe304b2908ceaa1f264e45ba2">ukf::srstate::ukf_param::no</a></div><div class="ttdeci">int no</div><div class="ttdoc">Dimension of the output : the measurements. </div><div class="ttdef"><b>Definition:</b> ukf_types.h:796</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_ac205dee75be43b32c76b0696d0182ab6"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#ac205dee75be43b32c76b0696d0182ab6">ukf::srstate::ukf_state::yi_prediction</a></div><div class="ttdeci">gsl_matrix * yi_prediction</div><div class="ttdef"><b>Definition:</b> ukf_types.h:835</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_afea69f7b0d410986ff5f5ca92d70545e"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#afea69f7b0d410986ff5f5ca92d70545e">ukf::srstate::ukf_state::sigmaPointMeasure</a></div><div class="ttdeci">gsl_vector * sigmaPointMeasure</div><div class="ttdef"><b>Definition:</b> ukf_types.h:846</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a0ab8c07796764941502c6a06f7d5b4dc"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a0ab8c07796764941502c6a06f7d5b4dc">ukf::srstate::ukf_state::wm_j</a></div><div class="ttdeci">gsl_vector * wm_j</div><div class="ttdef"><b>Definition:</b> ukf_types.h:851</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html_aedb04b04c2a74208f787e26eb3f406f5"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html#aedb04b04c2a74208f787e26eb3f406f5">ukf::srstate::ukf_param::lambda</a></div><div class="ttdeci">double lambda</div><div class="ttdoc"> </div><div class="ttdef"><b>Definition:</b> ukf_types.h:769</div></div>
<div class="ttc" id="namespaceukf_1_1math_html_a8123c498310baa12b0985a3bd271c06c"><div class="ttname"><a href="namespaceukf_1_1math.html#a8123c498310baa12b0985a3bd271c06c">ukf::math::signof</a></div><div class="ttdeci">double signof(double x)</div><div class="ttdef"><b>Definition:</b> ukf_math.h:45</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html_a03d77555a63ab31291ab4174babecfc2"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html#a03d77555a63ab31291ab4174babecfc2">ukf::srstate::ukf_param::n</a></div><div class="ttdeci">int n</div><div class="ttdoc">Size of the state vector. </div><div class="ttdef"><b>Definition:</b> ukf_types.h:781</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a9095ffca4f0bc5cbf7a42368cf7463d6"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a9095ffca4f0bc5cbf7a42368cf7463d6">ukf::srstate::ukf_state::xi</a></div><div class="ttdeci">gsl_vector * xi</div><div class="ttdef"><b>Definition:</b> ukf_types.h:829</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html_adcc3839a78bea1cd4822dc10bea265c0"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html#adcc3839a78bea1cd4822dc10bea265c0">ukf::srstate::ukf_param::prior_x</a></div><div class="ttdeci">double prior_x</div><div class="ttdoc">Prior estimate of the covariance matrix of the state. </div><div class="ttdef"><b>Definition:</b> ukf_types.h:801</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a70875ec1b32a4a73816c139ebe54d17e"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a70875ec1b32a4a73816c139ebe54d17e">ukf::srstate::ukf_state::Syi</a></div><div class="ttdeci">gsl_matrix * Syi</div><div class="ttdef"><b>Definition:</b> ukf_types.h:838</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a51c541f3e8ba1379cb5f61876e17c375"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a51c541f3e8ba1379cb5f61876e17c375">ukf::srstate::ukf_state::Svi</a></div><div class="ttdeci">gsl_matrix * Svi</div><div class="ttdef"><b>Definition:</b> ukf_types.h:833</div></div>
<div class="ttc" id="namespaceukf_1_1srstate_html_ada14e67760dda9e7a06a957a0a52a983"><div class="ttname"><a href="namespaceukf_1_1srstate.html#ada14e67760dda9e7a06a957a0a52a983">ukf::srstate::ukf_iterate</a></div><div class="ttdeci">void ukf_iterate(ukf_param &amp;p, ukf_state &amp;s, FunctProcess f, FunctObservation h, gsl_vector *yi)</div><div class="ttdoc">UKF-additive (zero-mean) noise case, &quot;Kalman Filtering and Neural Networks&quot;, p.233. </div><div class="ttdef"><b>Definition:</b> ukf_sr_state_ndim.h:234</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a478e32bd9fdb16834d784f6a30954510"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a478e32bd9fdb16834d784f6a30954510">ukf::srstate::ukf_state::temp_3n_n</a></div><div class="ttdeci">gsl_matrix * temp_3n_n</div><div class="ttdef"><b>Definition:</b> ukf_types.h:873</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_ab11872ef0fce0c3b1c02eb19e6198fb1"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#ab11872ef0fce0c3b1c02eb19e6198fb1">ukf::srstate::ukf_state::Ki_T</a></div><div class="ttdeci">gsl_matrix * Ki_T</div><div class="ttdef"><b>Definition:</b> ukf_types.h:858</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_add7a90f3736c8909a2e3abc3e4235ba7"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#add7a90f3736c8909a2e3abc3e4235ba7">ukf::srstate::ukf_state::ino_yi</a></div><div class="ttdeci">gsl_vector * ino_yi</div><div class="ttdef"><b>Definition:</b> ukf_types.h:837</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__param_html_aea53a3550db37bc15a3556df4c4edad8"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__param.html#aea53a3550db37bc15a3556df4c4edad8">ukf::srstate::ukf_param::gamma_aug</a></div><div class="ttdeci">double gamma_aug</div><div class="ttdef"><b>Definition:</b> ukf_types.h:776</div></div>
<div class="ttc" id="structukf_1_1srstate_1_1ukf__state_html_a90056d939d833550de52374098d3e348"><div class="ttname"><a href="structukf_1_1srstate_1_1ukf__state.html#a90056d939d833550de52374098d3e348">ukf::srstate::ukf_state::sigmaPointsMeasure</a></div><div class="ttdeci">gsl_matrix * sigmaPointsMeasure</div><div class="ttdef"><b>Definition:</b> ukf_types.h:847</div></div>
</div><!-- fragment --></div><!-- contents -->
<!-- start footer part -->
<hr class="footer"/><address class="footer"><small>
Generated by &#160;<a href="http://www.doxygen.org/index.html">
<img class="footer" src="doxygen.png" alt="doxygen"/>
</a> 1.8.6
</small></address>
</body>
</html>
